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ABSTRACT 


= Sequential extended Kalman filter and optimal 
smoothing algorithm was developed to provide real time 
estimates of torpedo position and depth on the three 
dimensional underwater tracking range at the Naval Torpedo 
Station, Keyport, Washington. The measurements consisted of 
acoustic pulse ο... from the torpedo to receiving 
array, which are nonlinear functions of the positions and 
the depth of the torpedo, were linearized and filter gains 
and filtered estimates of states calculated. By running the 
smoothing subroutine, all past filtered estimates of states 
and error covariance were smoothed. The program was tested, 
using simulated torpedo trajectories that traversed both 
single and multiple arrays, on an IBM-PC. The results showed 
that filter performance was dependent on system noise and 
the distance to the hydrophone array from the torpedo and 
the  smoothed estimates of states and error covariances were 


better than or equai to the filtered estimates. 
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I. INTRODUCTION 


The Naval Torpedo Station at Keyport, Washington 
currently operates two three-dimensional underwater tracking 
ranges utilizing a sonar transmitter installed in the 
torpedo to be tracked. The transmitter is synchronized with 
a master clock. Timed acoustic pulses are received by 
hydrophone arrays s then relayed via cable to a computer 
at the observation site. The computer calculates the 
positional coordinates of the torpedo and plots its 
trajectory through the water. 

The measured data, which consist of the elapsed time 
from transmission of a pulse until its receipt at the 
hydrophone array, is corrupted with noise due to combined 
effects of environmental factors and measurement 
instruments. 

The intention is to implement and test a sequential 
extended Kalman filter and smoothing routine which processes 
the transit times of the acoustic pulses and generates the 
filtered and smoothed estimates of the positions of tracked 
torpedo at a particular time. The design takes into account 


the elimination of the storage problem. 
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II. DESCRIPTION OF RANGE TRACKING GEOMETRY 


The hydrophone array, consisting of four independent 
elements, defines an orthogonal coordinate system in which 
transit time measurements sre made. AS shown in Figure 2.1, 
four  hydrophones X, Y, Z, and C are on four adjacent 
vertices separated ‚Бу a distance d, along the edge of the 
cube. The origin of the array coordinates is at the center 
of the cube with the orthogonal coordinates parallel to its 
edge. Positional information is computed from the transit 
times of a periodic synchronous acoustic signal traveling 
from the torpedo to the four hydrophones on the array. The 
.torpedoes are equipped with sonar transmitters which are 
transmitting an acoustic signal in every 1.31 seconds, 
within a range accuracy 3 to ЗО ft. When tracking by 
multiple arrays, the signal from the closest hydrophone 
array is defined as the basis for the time measurements and 
for the range calculations. A more detailed description of 


the range tracking capability is described in CRef. 1, 2]. 
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Figure 2.1 Geometry of a Tracking Array 
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I TS THEORY 


A. EXTENDED KALMAN FILTER 


The basic idea of the extended Kalman filter is to 
relinearize about each estimate X(k/k) , once it has been 


computed. As soon as a new state estimate is made, a new and 
better reference E trajectory is incorporated into the 
estimation process. (Ref. 3, 4, 51 

For the three-dimensional location problem three 


position states (X, Y, Z) and two velocity states SU s D 


specify target motion. The discrete linear and nonlinear 


observation equations are given by 


X(Ok*1) = 6. X Ck) +p. Wk) (5. 1) 
and 
Z(k) = h(X(k), k) + V(k) E ο 


where: $ and p are constant matrices; 


h is a nonlinear function of the state X 
(κ) is the plant excitation noise; 
V(k) is the measurement noise. 


In these equations the plant noise and measurement noise are 
assumed uncorrelated (white) with zero mean. That is, 


EG N CI 5 Q Oo owe 
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and 


Εεν(κ).ν «41 Ξ Rek) avt 


where: s = 1, k = j: 
= О, Ку j. 
In order to apply the linear filter, Equation 2.2 is 
expanded in a Taylor series about the best estimate of the 
state at that time and only the first order terms are kept. 


Equation 3.2 gives 


Z(k) = H(k) . X(k) + V(k) (SES 
where 
ah | 
H(k) = Saga | M. (3.4) 
d= İX(k) = X(k/k - 1) 


X(k/k — 1) is a predicted value of the state at time k, 


given the measurements until time k-1. 
A state error vector is defined by 

X (k/k) = X (k/k) - X(k), 
and a predicted state error vector is defined by 

X (k/k = > X (k/k — 1) < ку, 
The covariance of state error matrix is defined by 

P(k/k) = ECX (k/k).X ο». 
the predicted covariance of state error matrix is given by 


P(k/k — 1) πας κ ο. T ΙΙ; 


18 


The state excitation matrix is given by 


Qk) = p.ECWwtk) Wick dip! 


9 


and the measurement noise covariance matrix is 


R(k) = ECV(k).V (К) 1. 


The Kalman filter equations are given by tRef. 3, 4, 5J: 


~ 


Pik+i/k) = # P(k/l) $š + Otk) (3.5) 
GOOD SPOCK-1) HT GO CHOO POeZk-12H GO $R CO 273 «5.6) 
P(k/k) = CI - G(kO. HCGO J. P(Ok/k-1) ‘cum 
X(k+i/k) = # X(k/k) (5.8) 
Z(k/k-1) Ξ- Η(Χ(Κ/Κ-Ι), K) (5. 9) 


X(k/k) = X((k/k-1) + G(k) CZ(k) — Z(k/k-1)J (3.19) 


The @ matrix serves not only to allow for maneuvering 
but also to account for any model inaccuracies, that is, any 
discrepancies between the true action of the torpedo and its 
characterization by Equation 3.1. The Q matrix also serves 
to prevent the gain matrix Gtk) from approaching zero by 
always insuring uncertanity in the predicted covariance of 


error matrix P(k+1/k) CREF. 1, 5, 4, 51. 
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B. OPTIMAL SMOOTHING 

Smoothing 1S a non-real time data processing scheme that 
uses all measurements between O and N to estimate the state 
of a system at certain time k, where O < k < N. The smoothed 


estimate of X(k) Based on all measurements between O and N 


is denoted by X(k/N). The smoothed error covariance is 


denoted by P(kK/N) and P(k/N) < P(k/Z/k) means that the 


smoothed estimate of Atk) is at least 35 good as Ene 


filtered estimate or equal to its filtered estimate for all 
the time, except the terminal time. This is shown 
graphically in Figure 3.1. As portrayed in Figure 3.2, there 
are three classes of particular interest because of their 
applicability to realistic problems (Ref. 3, 4, 351. One is 
the Rauch~-Tung-Striebel form, which was chosen in our 
particular problem (Ref. 6, 73. 

The smoothed state estimate and the smoothed error 


covariance matrix are given by 


X(k/N) 2 X(k/k) * ACGOEXOCHIZND) — X(k+1/K)1 (3.11) 
X(k+1⁄k) = # X(k/k) (3.12) 
P(k/N) =P (k/K) +A (kK) EP (k+1/N) =P (k+1/k) 1A (k) Î Cap 
where 
r | 
Ας) = Β(Κ/Κ) δ P^ Gocl/k) for k < N. 
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Figure 3.1 Advantage of Performing Optimal Smoothing 
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Figure 3.2 Three types of smoothing: 


(a) 


t; 


xit, t) 


fixed-interval, 


(D) fixed-point, (c) fixed-lag smoothing. 


IV. PROBLEM DEFINITION 


A. OBSERVATION AND PLANT STATE EQUATIONS 
In the torpedo tracking problem, the non-linear 
observation equations are the four independent transit times 


from the tracked torpedo to the hydrophones, T_， το T and 


i Thus the non-linear measurement matrix is defined by 


Z¢k) = ГТ (ЕЮ) T (k) T (k) T TERU * VCk) (4.1) 
= C X y = EX 
where 
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Since the transit times аге readily available and 
non-linear functions of position, these equations can be 
linearized and Kalman filter theory applied using the 


extended Kalman filter. This procedure produces areal time 


tJ 
t) 


filtering on the transit times Ез s m and T , without 


the necessity of converting these times to positions. 
Equation 53.4 can be used to give the linearized 


observation matrix. When the derivatives are taken and 
evaluated at the predicted state values X(k/k-1) = X (k) the 


result is 
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The measurement noises, NV(k) s, are assumed to be 


zero-mean and independent with a covariance matrix 
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The plant state equations are 
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where X(k), Y(k) and Z(k) are the position coordinates of 


the torpedo at time tk), ο ο. and MS s are the X and Y 


components of the velocity. 


The excitation terms g, through g= are included to take 


into account the random changes in speed (YO ), heading 
€ 


Cy. 2, and depth Cys) which are assumed to be independent, 


et 


zero mean, rates of changes. Typical maneuvering parameters 


for the torpedo are given in L[Kef. 81. 


2 = 22 9/sec; T = = 1 
£ τ t 
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The effect of this excitation is to increase the predicted 
covariance of the state error matrix. 


The excitation covariance matrix is given by 


Q = p.ELW(k) Ww dol.p. (4.3) 
and 
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where the states are evaluated at the current state 


estimates  X(k/k). Substituting these expressions in the Q 


matrix results in 
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A more detailed derivation of the excitation covariance 
matrix is given in CRef. 8]. 

The excitation matrix serves not only to take into 
account the possibility of maneuvering, but of model 


inaccuracies as well. Q also used to prevent the gains of 


the filter from approaching zero as more and more data is 
processed, by insuring some uncertainty in the predicted 
SS GIES (Ref. 5. 4, 51. 


In the state form, the plant state equation is 


X(k+1) = 8 Xk) + p WK) (4.4) 
where: 
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B. DEFINITION OF MULTIPLE ARRAY TRACKING 

The coordinate system is defined as shown in Figure 4.1. 
These T2 positions, an XYZ position for each of 4 
hydrophones in ὁ arrays, are placed into a 6 x= 12 matrix 
HYDRO and referenced throughout the program. The torpedo 
position is referenced to a central level rectangular 
coordinate system. The non-linear observation equations 


become 


ο = ET Ck) T Ck) T (k) T TD + V (k) (4.5) 
= c x y Ζ = 


where 
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and the subscripted variables X, Y, and Z are the 


coordinates of a particular array being used. 
The decision parameter used to determine the switching 


from array to array is a Straight handoff. If the predicted 


A 


ee: 9 е greater than 3,000 feet from the 


x position, 


array in use, then index is incremented and the next row of 
HYDRO is implememted. This placed into the program the X, Y, 
and Z positions of the hydrophones in the next array. The 
handoff can easily be utilized in real range operations, as 
the transit times from adjacent arrays are present at the 


computer for a particular time slot. 
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a) Coordinate System for Multiple Array Tracking 
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D ) Hydrophone Array Location Matrix 


Figure 4.1 Geometry of Multiple Array Tracking 


C. SEQUENTIAL EXTENDED KALMAN FILTER 

In the sequential approach, after modifying the basic 
Kalman filter equations, calculations are performed on each 
of the four independent transit times in the following 


order: του pen um and T_ for each 1.31 second time slot. 


Since the four transit times are independent and processed 
sequentially, the covariance of error matrix and the state 
vector . are updated four times during each time slot. Thus 
more accurate estimates of the filter states are achived. 
Modification of the filter equations for the sequential 
approach circumvented the matrix inversion in the gain 
equation. An invalid transit time measurement will result in 
the filter ignoring the update information -for that 


particular measurement only. 


The estimate of the states X(k/k) , based on one transit 


time measurement are used as the prediction X(k/k-1) for the 


calculationson the next measurements. Thus for the first 


time measurement p only the first row of the linearized H 


matrix is calculated and then the first gain column 


corresponding to the first time measurement ТЕ is calculated 


Dy 
POk/k-—1) Hi n 
ies] " CC 83508 ERN E (4.6) 
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where 1 = 1 to 4 corresponding to the four measured transit 
times. 
An estimate of the particular observation time is 


calculated by using Equation 3.9 evaluated at the predicted 
state X(k/k-1). The difference between observed transit 


times and the estimated transit times forms the residual 


which is used in the estimate equation 


X . 2 X(k/k-1) * G. [Residual ] (4.7) 
_ 1 _ icol 


This equation gives an estimate of the states based on one 
of the four time measurements. 
The covariance of error is calculated based on one 


measurement by 


P. = [I - G. H. ] P. (4.8) 
1 icol lir Ow 1 =1 


where: I is identity matrix; 


ae is the covariance matrix calculated from the 


previous transit time measurement or if i = 1, 

the predicted error covariance P(k/k-1). 
Editing erroneous time measurement is achieved by 
implementing a three sigma gate using the covariance of the 


measurent noise (R) and the covariance of the estimation 


error F(k/k).The gate then is written for each time 


measurement i = 1 to 4: 
= : 2 2152 
gate = 3* CE CP, ;maximum) / (4860. ) J te πι, 2 (4. 9) 
where j = 1, 3, S. The gate expands or decreases depending 


on the confidence level of the position estimate and the 
transit time. If the difference between the actual transit 
time received and predicted transit time to a particular 
hydrophone exceeds the gate, the measurement is considered 
unacceptable and the filter gain is set to zero causing the 


filter to ignore the data and take the prediction of the- 
states as the estimate X(k/k) 9 X(k/k-1). 


Bounding the residual bias error is achieved by making 
comparison between the average of the absolute value of the 
time residuals and the preset threshold. If the average of 
the time residuals exceeds the preset threshold, Ü is 
calculated and added to the last updated covariance of error 
matrix P. Then filter reiterates the gain, covariance, and 
state estimate equations for the same time slot. This 
procedure continues until the average of the time residuals 
falls below the preset threshold at which time an acceptable 


state vector estimate has been obtained for the time slot. 


CA 
hJ 


D. OPTIMAL SMOOTHING ALGORITHM 

The smoothing solution starts with the filtered estimate 
at the last point and calculates backward point by point 
determining the smoothed estimate as a linear combination of 
the filtered estimate at that point and the smoothed 
estimate at the previous point (Ref. 6]. 

It can be seen from the error covariances that the 


filter has reached-a steady-state condition by the end of 


the forward sweep. As an example, let us enter the backward 


^. 


sweep at the end point where k = 20. Here we have X (20/20) 


and P(20/20). Since the filter solution at this point is 
conditioned on all the measurement data, it is also the 
smoothed estimate at k = N = 20. We are now ready to compute 
the smoothed estimate one step back at κ = 15. From 


Equations 3.11, 3.12 and 3.135 we have 


tT. 


X(19/20) = X 19/19) + A(19) EX C20/20) ΚΘ} 
stored stored 


X(20/19) = # X(19/19) 
stored 


A(19) = P(19/19) & P^ (20/19) 
stored stored 


(А 
(А 


(Ισ το το ο) 


+ AC19) EP (20/20) - P(20/19)1 A 
stored 


(19) 
stored stored 


and to compute the smoothed estimate two step back at k = 18 


X(18/20)= X(18/18)+ AC18)CX(19/20) - X(19/18) 3 
stored 


X(19/18) = B xX (18/18) 
stored 


Α(18) - Ρ(18/18) 5 ϱ (19/18) 


stored stored 


P(18/20) = P(18/18) + AC1IB) CP(19/20) - P619/198) ] αἱ (18) 
stored 


stored 


This procedure continues until the time k reaches to 1. 


V. SIMULATION RESULTS 

A. MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 

The true trajectory of the torpedo is a straight line 
with a SO ft/sec velocity toward the origin of hydrophone 
array parallel to X-axis, drawing two tangent circles with 
10 deg/sec turn rate, in the horizantal X-Y plane through a 
multiple array. 

In the first part of this run, the initial position of 
the torpedo is 38000 ft in X, 7000 ft in Y, and 300 ft in Z. 
Figures 5.1 and 5.2 depict the filtered and smoothed 
estimate of the trajectory, with zero initial velocity 
errors and 25 ft initial position errors in X and Y. The 
errors in the filtered and the smoothed estimate of 
positions in X, Y and Z are drawn in Figures 3.5, 5.4, 5.5, 
S.6, 5.7 and 5.8. For the “Kalman filter, errors ranged 
between -1.2 and 2.6 ft in X, -5.9 and 1.9 ft in Y, O.1 and 
2.9 ft in Z. After smoothing, the errors occured in smaller 
range, which is, between -1.4 and 2.4 ft in X, -5.0 and 1.9? 
ft. in Y, O.1 and O.7 ft in Z. The diagonal terms of the 
filtered and smoothed error covariance matrices are shown 
ΕΘΗ in Figures 3:9, S.10, a.11, 5.12, 3.13, 5-14. 


5.15, 5.16, 5.17, 5.18. 
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In the second part of this run, the initial position of 
the torpedo is SSOOO ft in X, 7000 ft in Y, and S00 ft in Z. 
The filtered and smoothed estimate of the trajectory are 
drawn in Figures 5.19 and 5.20. Taking this different 
initial geometry made the errors in the position of the 
torpedo to take place in bigger values during first time 
slot of the filtering and last time slot of the smoothing. 
As seen in Figures 5.21, 5.22, 5.25, να, οσο --Ἡ- 
errors ranged between -16.3 and 1.9 ft in X, -1S5.1 and 4.6 
ft in Y, -5.0 and 1.0 ft in Z for the Kalman filter and for 
the smoothing this error range is between -12.6 and 1.3 ft 
in X, —12.5 and 4.8 ft їп ҮҮ, 1:597 angel. 0 tcn. 
diagonal terms of the filtered and smoothed error covariance 
matrices displayed slightly different magnitude, as seen in 
Figures 9.2/7, 9.28, S.29, 5.50. 5.51 5.52 ο ον πο 20 Ns 


and 5. 56. 


B. MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN 

In this run, the true trajectory of the torpedo is a 
straight line with a 50 ft/sec velocity toward the origin of 
hydrophone array parallel to X-axis in the horizantal X-Y 
plane through a multiple array. 

With the initial position of the torpedo is 38000 ft in 
X, /OOO ft in Y, and SOO ft in Z. The filtered and smoothed 


estimate of the trajectory, with zero initial velocity 


errors and 25 ft initial position errors in X and Y, are 
depicted in Figures 5.37 and 5.38. Figures 5.39, 5.40, 5.41, 
3.42, 5.43 and 5.44 give the errors in the filtered and the 
smoothed estimate of positions in X, Y and Z. For the Kalman 
filter, errors ranged between -1.6 and 2.6 ft in X, -5.9 and 
4.7 ft in Y, -0.2 and 2.5 ft in Z. After smoothing, the 
errors occured in smaller range, which 15, between -1.1 and 
2.4 ft in X, -5.0 and 1.7 ft in Y, -0.2 and O.6 ft in Z. The 
diagonal terms of the filtered and smoothed error covariance 


matrices are shown pictorially in Figures 5.45 through 5.54. 


C. SINGLE ARRAY ADAPTIVE MANEUVERING RUN 

The previous tests described the filter: and smoothing 
performance for both straight and maneuvering runs through 
multiple array. Using the same basic torpedo trajectories as 
in multiple array, Similar tests are performed for 
maneuvering run through single array.During the single array 
tracking, the initial position of the torpedo is 73500 ft in 
X, 1300 ft in Y and O ft in Z, which gives different initial 
geometry. The filtered and smoothed estimates of the 
trajectory and the corresponding position errors in X, Y and 
Z are pictorially given in Figures 5.55 through 5.62. For 
the Kalman filter, errors ranged between -1.6 and 3.5 ft in 


X, -19.1 and 8.9 ft in Y, -O.3 and 1. ft in Z. After 


smoothing, the errors occured in smaller range, which is, 


between -—1.1 and 3.1 ft in XK, —17.7 апас о το e 
and 1.6 ft in Z. The diagonal terms of the filtered and 
smoothed error covariance matrices are shown pictorially in 


Figures $5.63 through 3.72. 


D. SINGLE ARRAY STRAIGHT RUN 

The purpose of this last series of tests is to 
functionally demonstrate the performance of the filter and 
smoothing during a Stone run through single array using 
the same initial torpedo position as in single array 
adaptive maneuvering run. The filtered and smoothed 
estimates of the trajectory and the corresponding position 
errors in X, Y and Z are pictorially given in Figures 3.75 
through 3.80. For the Kalman filter, errors ranged between 
-1.6 and 3.3 ft.in X, -19:1 and 8:9 ft in os ando 
in Z. After smoothing, the errors occured in smaller range, 
which is, between -O.6 and 3.1 ft in X, -17.9 and 3.5 ft in 
Y, -0.2 and O.7 ft in Z. The diagonal terms of the filtered 
and smoothed error covariance matrices are shown pictorially 


in Figures 3.80 through 3. 70. 


VI. CONCLUSIONS 


The sequential extended Kalman filter and smoothing 
routine sufficiently generated the filtered and smoothed 
estimates of the states, which specify the motion of the 
torpedo. Errors generated by running the routine on the 
IBM-PC are comparable to those given in the previous search, 
which was done on’a large IBM computer tRe£. ΤΙ, 


In the smoothing problem, computing the predicted 


estimates of the states, X(k*l/k), from the estimates of the 


states, X(k/k), eliminates the storage problem for X(k+i/k). 


In future studies, an algorithm for computing P(k/k) from 
P(k+1/k+1) and hence eliminating the storage problem for 
P(k/k), should be invesigated. 

Examining the errors and their covariances, it is 
evident that the uncertainty in position exist only in the 
Y direction for the case where the torpedo is moving along 
the X axis. The results of the straight run analyses show 
that the propagation of the filtered error covariance is 
dependent on the path of the torpedo with respect to 
hydrophone array. Upon observing the error propagation it is 
apparent that the position errors exhibit approximately 


equal oscillations about zero indicating that the 


measurement noise 45 the dominant error source driving the 
filter. 

The smoothed estimates of the states are at least as 
good as or better than the filtered estimates. The filter 
performance was dependent on system noise and the distance 
from the torpedo to the hydrophone array. Errors get bigger 
as the torpedo approaches the tracking limit of the 
hydrophone array. 

Additional work should be done using trajectories 
generated from actual torpedo runs on the Dabob test range. 
The rotation and reduction of the error ellipsoids should 
be also included in future studies. 

The filter should be of use in range safety in warning 
for possible collisions. Also it may prove invaluable in 
torpedo recovery when there is a malfunction and the torpedo 


1s sometimes buried in many feet of mude. 
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MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 


FILTERED ESTIMATE OF TRAJECTORY WITH NOISE | 
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Figure 5.1 Filtered Estimate of Trajectory of the Torpedo 
During a Maneuvering Run through Multiple Array 


MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
SMOOTHED ESTIMATE OF TRAJECTORY WITH NOISE 
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Figure 5.2 Smoothed Estimate of Trajectory of the Tarpedo 
During a Maneuvering Run through Multiple Array 
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Figure 5.3 Error in Filtered Estimate of Position in X of 
the Torpedo During a Maneuvering Run through Multiple Array 
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Figure 5.4 Error in Smoothed Estimate of Position in X of 
the Torpedo During a Maneuvering Run through Multiple Array 
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Error in Filtered Estimate of Position in Y of 
During a Maneuvering Run through Multiple ñrray 
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Error in Smoothed Estimate of Position in Y of 
During a Maneuvering Run through Multiple Array 
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Figure 5.7 Error in Filtered Estimate of Fosition in Z of 
the Torpedo During a Maneuvering Run through Multiple Array 
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Figure 5.8 Error in Smoothed Estimate of Position in Z of 
the Torpedo During a Maneuvering Run through Multiple Array 
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MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
FILTERED ERROR COVARIANCE P(K/K) WITH NOISE 
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Figure 3.9 Variance of Filtered Position Error in X of the 
Torpedo During a Maneuvering Run through Multiple Array 
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Figure 3.10 Variance of Smoothed Position Error in X of the 
Torpedo During a Maneuvering Run through Multiple Array 
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Figure 5.11 Variance of Filtered Velocity Error in X of the 
Torpedo During a Maneuvering Run through Multiple Array 
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Flgure 5.12 Variance of Smoothed Velocity Error in X of the 
Torpedo During a Maneuvering Run through Multiple Array 
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Figure 5.15 Variance of Filtered Position Error in Y of the 
Torpedo During a Maneuvering Run through Multiple Array 
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Figure 5.14 Variance of Smoothed Position Error in Y of the 
Torpedo During a Maneuvering Run through Multiple Array 
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MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN 
FILTERED ERROR COVARIANCE P(K/K) WITH NOISE 


2 


O 
= 
x 


PCK/KIJ (4,4) 
( (FT/SEC) xx2) 


40 60 80 
TIME ΠΠ 





Figure 3.15 Variance of Filtered Velocity Error in Y of the 
Torpedo During a Maneuvering Run through Multiple Array 
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Figure 3.16 Variance of Smoothed Velocity Error in Y of the 
Torpedo During a Maneuvering Run through Multiple Array 
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Figure 5.17 Variance of Filtered Position Error in Z of the 
Torpedo During a Maneuvering Run through Multiple Array 
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Figure 5.18 Variance of Smoothed Position Error in Z of the 
Torpedo During a Maneuvering Run through Multiple Array 
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Figure 5.19 Filtered Estimate of Trajectory of the Torpedo 
During a Maneuvering Run through Multiple Array 
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Figure 5.20 Smoothed Estimate of Trajectory of the Torpedo 
During a Maneuvering Run through Multiple Array 
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Figure 3.21 Error in Filtered Estimate of Position in X of 
the Torpedo During a Maneuvering Run through Multiple Array 
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Figure 5.22 Error in Smoothed Estimate of Position in X of 
the Torpedo During a Maneuvering Run through Multiple Array 
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Figure 3.23 Error in Filtered Estimate of Position in Y of 
the Torpedo During a Maneuvering Run through Multiple Array 
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Figure 3.24 Error in Smoothed Estimate of Fosition in Y of 
the Torpedo During a Maneuvering Run through Multiple Array 
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Figure 5.25 Error in Filtered Estimate of Position in Z of 
the Torpedo During a Maneuvering Run through Multiple Array 
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Figure 3.26 Error in Smoothed Estimate of Position in Z of 
the Torpedo During a Maneuvering Run through Multiple Array 
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Figure 5.27 Variance of Filtered Position Error in X of the 
Torpedo During a Maneuvering Run through Multiple Array 
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Figure 5.28 Variance of Smoothed "osition Error in X of the 
Torpedo During a Maneuvering Run through Multiple Array 
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Figure 3.29 Variance of Filtered Velocity Error in X of the 
Torpedo During a Maneuvering Run through Multiple Array 
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Figure 5.30 Variance of Smoothed Velocity Error in X of the 
Torpedo During a Maneuvering Run through Multiple Array 
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Figure 5.31 Variance of Filtered Position Error in Y of the 
Torpedo During a Maneuvering Run through Multiple Array 
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Figure 53.32 Variance of Smoothed Fosition Error in Y of the 
Torpedo During a Maneuvering Run through Multiple Array 
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Figure 5.35 Variance of Filtered Velocity Error in Y of the 
Torpedo During a Maneuvering Run through Multiple Array 
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Figure S.54 Variance of Smoothed Velocity Error in Y of the 
Torpedo During a Maneuvering Run through Multiple Array 
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Figure 3.35 Variance of Filtered Position Error in Z of the 
Torpedo During a Maneuvering Run through Multiple Array 
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Figure 3.36 Variance of Smoothed Position Error in Z of the 
Torpedo During a Maneuvering Run through Multiple Array 
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Figure 5.37 Filtered Estimate of Trajectory of the Torpedo 
During a Straight Run through Multiple Array 
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Figure 3.38 Smoothed Estimate of Trajectory of the Torpedo 
During a Straight Run through Multiple Array 
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Figure 3.39 Error in Filtered Estimate of Position in X of 
the Torpedo During a Straight Run through Multiple Array 
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Figure 5.40 Error in Smoothed Estimate of Position in X of 
the Torpedo During a Straight Run through Multiple Array 
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Figure 3.41 Error in Filtered Estimate of Position in Y of 
the Torpedo During a Straight Run through Multiple Array 
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Figure 5.42 Error in Smoothed Estimate of Position in Y of 
the Torpedo During a Straight Run through Multiple Array 
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Figure 5.43 Error in Filtered Estimate of Position in Z of 
the Torpedo During a Straight Run through Multiple Array 
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Figure S.44 Error in Smoothed Estimate of Position in Z O£ 
the Torpedo During a Straight Run through Multiple Array 
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Figure 5.46 
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Variance of Smoothed Position Error in X of the 
During a Straight Run through Multiple Array 
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Figure 3.47 Variance of Filtered Velocity Error in X of the 
Torpedo During a Straight Run through Multiple Array 
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Figure 5.48 Variance of Smoothed Velocity Error in X of the 
Torpedo During a Straight Run through Multiple Array 
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Figure 5.49 Variance of Filtered Position Error in Y of the 
Torpedo During a Straight Run through Multiple Array 
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Frowre 5.50 Variance of Smoothed Position Error in Y of the 
Torpedo During a Straight Run through Multiple Array 


MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN 
FILTERED ERROR COVARIANCE P(K/K) WITH NOISE 


2 


© 
--- 
x 


РГК/КЛИЧ,Ц) 
((ЕТ/5ЕС) жж2) 


20 40 60 80 
TIME το 





Figure 5.51 Variance of Filtered Velocity Error in Y of the 
Torpedo During a Straight Run through Multiple Array 
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Figure 5.52 Variance of Smoothed Velocity Error in Y of the 
Torpedo During a Straight Run through Multiple Array 
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Figure 3.53 Variance of Filtered Position Error in Z of the 
Torpedo During a Straight Run through Multiple Array 
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Figure 3.34 Variance of Smoothed Position Error in Z of the 
Torpedo During a Straight Run through Multiple Array 
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Figure 5.55 Filtered Estimate of Trajectory of the Torpedo 
During a Maneuvering Run through Single Array 
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Figure 5.56 Smoothed Estimate of Trajectory of the Torpedo 
During a Maneuvering Run through Single Array 
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Figure 3.57 Error in Filtered Estimate of Position in X of 
the Torpedo During a Maneuvering Run through Single Array 
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Figure 5.58 Error in Smoothed Estimate of Position in X of 
the Torpedo During a Maneuvering Run through Single Array 


69 


SINGLE ARRAY ADAPTIVE MANEUVERING RUN 
ERROR IN FILTERED ESTIMATE WITH NOISE 


e 


10.0 


(FT) 


© 
© 
c 
o 
a o 
= ic 
(Сс ἡ 
и) 
c 
=> © 
o 
e 
I 


20 40 60 80 
TIME“ SCOTS 





Figure 5.5? Error in Filtered Estimate of Position in Y of 
the Torpedo During a Maneuvering Run through Single Array 
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Figure 3.60 Error in Smoothed Estimate of Position in Y of 
the Torpedo During a Maneuvering Run through Single Array 
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Figure 3.61 Error in Filtered Estimate of Position in Z of 
the Torpedo During a Maneuvering Run through Single Array 


SINGLE ARRAY ADAPTIVE MANEUVERING RUN 
ERROR IN SMOOTHED ESTIMATE WITH NOISE 


ο 
e 
e 


ce 
zh 


(FT) 


= 


ας 
© 
ac 
= 
lJ 
n4 


20 80 60 80 
TINE ΠΠ 





Figure 3.62 Error in Smoothed Estimate of Position in Z of 
the Torpedo During a Maneuvering Run through Single Array 
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Figure 5.63 Variance of Filtered Position Error in X of the 
Torpedo During a Maneuvering Run through Single Array 
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Figure 5.64 Variance of Smoothed Position Error in X of the 
Torpedo During a Maneuvering Run through Single Array 
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Figure 5.645 Variance of Filtered Velocity Error in X of the 
Torpedo During a Maneuvering Run through Sinlge Array 
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Figure 3.66 Variance of Smoothed Velocity Error in X of the 
Torpedo During a Maneuvering Run through Single Array 
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Figure 5.67 Variance of Filtered Position Error in Y of the 
Torpedo During a Maneuvering Run through Single Array 
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Figure 5.68 Variance of Smoothed Position Error in Y of the 
Torpedo During a Maneuvering Run through Single Array 
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Figure $5.69 Variance of Filtered Velocity Error in Y of the 
Torpedo During a Maneuvering Run through Single Array 
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Figure 3.70 Variance of Smoothed Velocity Error in Y of the 
Torpedo During a Maneuvering Run through Single Array 
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Figure 5.71 Variance of Filtered Position Error in Z of the 
Torpedo During a Maneuvering Run through Single Array 
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Figure 5.72 Variance of Smoothed Position Error in Z of the 
Torpedo During a Maneuvering Run through Single Array 
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Figure 3.75 Filtered Estimate of Trajectory of the Torpedo 
During a Straight Run through Single Array 
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Figure 5.74 Smoothed Estimate of Trajectory of the Torpedo 
During a Straight Run through Single Array 
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Figure 5.75 Error in Filtered Estimate of Position in X of 
the Torpedo During a Straight Run through Single Array 
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> Figure 5.76 Error in Smoothed Estimate of Position in X of 
the Torpedo During a Straight Run through Single Array 
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Figure 3.77 Error in Filtered Estimate of Position in Y of 
the Torpedo During a Straight Run through Single Array 
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Figure 3.78 Error in Smoothed Estimate of Position in Y of 
the Torpedo During a Straight Run through Single Array 
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Figure 5.79 Error in Filtered Estimate of Fosition in Z of 
the Torpedo During a Straight Run through Single Array 
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Figure 5.80 Error in Smoothed Estimate of Position in Z of 
the Torpedo During a Straight Run through Single Array 
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Figure 5.81 Variance of Filtered Position Error in X.of the 
Torpedo During a Straight Run through Single Array 


SINGLE ARRAY ADAPTIVE STRAIGHT RUN 
SMOOTHED ERROR COVARIANCE P(K/N) WITH NOISE 


x10! 
60.00 


40.00 


PEKZNJ(1,1) 
(FTxx2) 
0.00 20.00 


0 20 40 60 80 100 120 
ΤΗΕ, SEIS 


Figure 5.82 Variance of Smoothed Position Error in X of the 
Torpedo During a Straight Run through Single Array 
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SINGLE ARRAY ADAPTIVE STRAIGHT RUN 
FILTERED ERROR COVARIANCE P(K/K) WITH NOISE 
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Figure 5.85 Variance of Filtered Velocity Error in X of the 
Torpedo During a Straight Run through Single Array 
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Figure 3.84 Variance of Smoothed Velocity Error in X of the 
Torpedo During a Straight Run through Single Array 
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Figure S.85 Variance of Filtered Position Error in Y of the 
Torpedo During a Straight Run through Single Array 
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Figure 3.86 Variance of Smoothed Position Error in Y of the 
Torpedo During a Straight Run through Single Array 
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Figure 5.87 Variance of Filtered Velocity Error in Y of the 
Torpedo During a Straight Run through Single Array 


SINGLE ARRAY ADAPTIVE STRAIGHT RUN 
SMOOTHED ERROR COVARIANCE P(K/N) WITH NOISE 
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Figure 5.88 Variance of Smoothed Velocity Error in Y of the 
Torpedo During a Straight Run through Single Array 
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Figure 3.89 Variance of Filtered Position Error in Z of the 
Torpedo During a Straight Run through Single Array 


Figure 3.90 Variance of Smoothed Position Error in Z of the 
Torpedo During a Straight Run through Single Array 
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APPENDIX A 
PROGRAM DESCRIPTION 





A. GENERAL 

The sequential extended Kalman filter and Smoothing 
routine is described in detail by CRef. 13. Implementation 
is done by using FORTRAN77 compilers on IBM-PC. CRef. 10, 


011. 19 ^ PM 


B. RUNNING THE PROGRAM ON THE IBM-FC 

These directions apply for the IBM-FC computer or other 
computers(compatibles) with two floopy disk-drives, 640K 
memory, color/graphic board, math coprocessor and paraiel 
dot matrix printer or printer/plotter. The software utilized 
during the simulation studies are: 

1. Operating System DOS 2.10 with required fiies to 
create Virtual disk and full screen editor 
utilities. 

г IBM Professional FORTRAN Compiler 1.00. 

X. “Microsoft FORKIFKSN77 S 209: 

4. Plotworks PLOT88.LIB. 

S. Source files. 

Getting the sequential extended kalman filter and 

smoothing routine started 15 essentially a five step 


process: start your computer; edit the source file and make 


required changes and then compile: run the executable file 
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and get the data to be available for plotting routine: edit 
the source file of plotting routine and make the necessary 
changes for plotting titles and then compiles run plotting 
routine. Start the computer up with an operating system and 
get the program running simply by typing "RUN", which is 


given in Appendix E, at promt "A>". 
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APPENDIX B 


SEQUENTIAL EXTENDED KALMAN FILTER AND OPTIMAL SMOOTHING 


PROGRAM LISTING 


PROGRAM THESIS 


REAL *8 
REAL. *8 
REAL *8 
REAL *8 
REAL *8 
REAL *8 
REAL *8 
REAL *8 
REAL *8 
REAL *8 
REAL *8 
REAL #8 
REAL *8 
REAL *8 
REAL *8 
REAL *8 
REAL *8 
REAL *8 


XKKM1(5) ,PKKM1 (5,5) ,PHI (5,58) , GOMMA (5,3) ,GATE 
GAMMAT (3,5) ,COVW (3,3) ,COVV (4,4) ,QOTEMP (5,2) ,F 
TRUX (121) , TRUY (1210 , TRUZ (121) ,ZI (4) , HROW(S) 
ZHAT ,GDÉNOM, GDTEMP , PDUM (5,5) ,PI (5,59) ,WHTN,A14 
ZDIFF (4) ,ZIC(4) ,XI (5) ,XKK(S) , PKK (S,5) , ZDIFAV 
DATR(17) ,WINIT,PHIPKK (5,5) ,PKTEMP (5,5) ,SIGACC 
SIGDIV,SIGCC, XKERR (121) , YKERR(121) ,XP6(5,121) 
HYDRO(56,12) ,XBCA) , YB(CA) , ZBCA) , XSERR (1212 , TD C2) 
SIGCCC,SIGAAC,SIGDDI,XP(5,121) , SMTH(121) ,GI (5) 
PS(121,5,5),881(121,5,5) ,P1(121,5,5) ,0(S,5) 
YSERR (121) ,ZSERR (121) ,GNUM(S) ,PHIT(5,5) ,XP1(5) 
ZDIFTO,CH(S,5) , TEMP1 (5,5) ,XNNM1 (S) , TEMP2 (5) 

AK (5,5) ,AKT (5,5) , TEMP3(S) , TEMP4 (5,5) , XKKS (5) 
PNNM1 (5,5) , TEMPS(5,5) , TEMP6(5,5) , PKKS(5,5) 
SS2(5) ,P2(5,5) ,S53(5,5) ,SS3R(5,5) ,SIG | 
ZKERR (121) ,X1KERR, X2KERR, YIKERR, Y2KERR, Zi KERR 
Z2KERR , X1SERR, X2SERR, YISERR, Y2SERR, Z1SERR 
Z2SERR 


C COORDINATES OF HYDROPHONE ARRAY, FOR MULTIPLE ARRAY 

DATA HYDRO/36000. , 30000. , 24000. , 18000. , 12000. , 6000. 
,6*6000.,6*0.0,36030. , 30030. , 240350. , 18030. , 12030. 
,6030. ,6*6000. ,6*0.0,36000. , 30000. , 24000. , 18000. 
,12000. ,6000. ,6*6030. ,6*0.0,26000. , 30000. , 24000. 
,18000.,12000. ,7*6000. ,6*20. / 


DATA 
DATA 
DATA 
DATA 
DATA 


FOR 
DATA 


PKKM1/1000.0,5*0.0,1000.0,5*0.0,1000.0,5*0.0 


,1000.0,5*0.0,1000.0/ 


PHI/1.,4*0.,1.21,1.,59*0.,1.,4*0. ,1. 21,1. ,9*0. 


1 


GAMMA/0.858,1.31,5*0.0,0.958,1.31,5*0.0,1.31/ 
COVW/1.0,3*0.0,1.0,3*0.0,1.0/,WINIT/O. 49/ 
COVV/1.0D-8,4*0.0,1.0D-8,4*0.0,1.0D-8,4*0.0 


; L 9D-87 


MULTIPLE ARRAY TRACKING 
DATR/38000.,7000.,300. ,-50. ,0. , «0. , 3«Q. 


-,4.712389,.1745229,.1,8.1,600. ,800. / 
DATA XKKM1/37975. 0,-50.0,6975.0,0.0,300. 0/ 


+ 
+ 
+ 
+ 

C 
+ 
+ 
+ 

C DATA 

C 

C 

C 

C 


SECOND DATA FOR MULTIPLE ARRAY TRACKING 


DATA DATR/35000.,7000.,300.,-50. ,0. , 3*0. , 3*0. 
-,4.712389,.1745329,.1,8.1,600. ,800. / 
DATA XKKM1/34975.0,—-50.0,6975.0,0.0,300. 0/ 


ag 


FIRST DATA FOR SINGLE ARRAY TRACKING 
DATA DATR/7500.0,1300.0,0.0,-50.0,0.0,3*0.0,3*0.0 
4,4.712389,.1745329,0.1,8.1,600.0,800.0/ 
DATA XKKM1/7475.0,-50.0,1275.0,0.0,0.0/ 
SECOND DATA FOR SINGLE ARRAY TRACKING 
DATA DATR/2000.0,1000.0,300.0,-50.0,0.0,3*0.0,3*0.0 
-€,4.712389,.1745329,0.1,8.1,600.0,800.0/ 
DATA XKKM1/1975.0,—-50.0,975.0,0.0,300.0/ 
DATA FOR SUBROUTINE GFIND 
DATA SIGACC/36.2/,S8IGDIV/1.0/,S81GCC/22.2/ ,NZDIFF/4/ 
DATA II/1/,IJ/2/,IK/3/,IL/4/,IM/S/,MINE/1/,JTIME/119/ 
C OPEN STATEMENTS FOR OUTPUT FILES 
OPEN (4,FILE='TRUDI.DAT’) 
OPEN (13,FILE=’PKK. DAT’) 
OPEN (11,FILE=‘PKN. DAT’) 
OPEN (7,FILE=’XKK. DAT’) 
OPEN (8,FILE=‘'XKN. DAT’) 
OPEN (9, FILE=’XKERR. DAT’) 
OPEN (10,FILE=‘' XSERR. DAT’) 
OPEN (12, FILE=’OUTPUT. DAT’) 


(000000000 


SIGCC = (SIGCC * 2.141592654) / 180.0 
C TRANSPOSE OF GAMMA MATRIX 
CALL TRANS (GAMMA, IM,IK,GAMMAT) 
TRANSPOSE OF PHI MATRIX 
CALL TRANS(PHI,IM,IM,PHIT) 
USE THESE STATEMENTS TO CALCULATE CONSTANT ϱ - MATRIX 
CALL PROD (GAMMA,COVW,IM,IK, IK,QTEMP) 
CALL PROD (QTEMP ,GAMMAT,IM,IK,IM,Q) 
ITIME = JTIME + 1 
C USE THIS STATEMENT FOR MULTIPLE ARRAY TRACKING 
I7 = O 
(29m dle e Ie o ote te e De o جد‎ e ae 3 جد جد جد 3£ 3 3 3 3 3£ جد‎ AE XC At do dE H^ JEJE de JE جد جد‎ JC HC JE JC JE 3 جد چب‎ e e e e e e e e € E Rn 
C TIME SLOTS START HERE 
DO 128 KK = 1 , ITIME 
WRITE (#,562) KK 
562 FORMAT(/,10X,‘TIME SLOT IN FILTERING :‘,I5) 
C 
C 3 Be e Le Be e Le Se Be Be e Le Be Be Be Be Be o Se Le e Ke Heke Ge BRR HR eH VR HHH HK o Be Se Re e Re B o S εδ δε δε e 
C CHOSE THE HYDROPHONE ARRAY FOR MULTIPLE ARRAY TRACKING 
IF(XKKM1(1).GE. 33000.0) I8 - 1 
IF((XKKM1 (12. GE. 27000. 0) . AND. CXKKM1 C12 . LT. 33000.0)) 


Ω 


O0 O0 €) 


* 18 = 2 

IF ((XKKM1 (1) .GE.21000.0) . AND. (XKKM1 (1) -LT.27O00.0)) 
ste Ig = 5 

IF ((XKKM1 (01) .GE.15000.0) . AaND. (XKKM1 (1) .LT.21000.0)) 
τ 18 = 4 

IF ((XEKM1 (1) . GE. 9000. O0) . AND. (XKKM1 (1) .LT. 15000. 0) ) 
+ Ig = 3 


IF (XKKM1 (1) LT.9000.0) 18 6 
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14 = Š IS 
I9 --14-- 
IS = 14 - 1 
ΧΒ(15) = HYDROCIS,1IS) 
YB(IS) 2 HYDRQO(CIS8,I16) 
ZB(IS) 2 HYDRO(ISG,I4) 


205 CONTINUE 

C WRITE THE COORDINATES OF CHOSEN HYDROPHONE ARRAY 
ΙΕ(Ι7.ΝΕ. 18) THEN 
WRITE(*,217) IB,KK 


21 FORMAT (/,10X, ‘ARRAY ',I2 
+ ,' STARTS TRACKING AT TIME ’ 15) 
WRITE (*,2145) KK,I8, (I3,XB(I3) ,YB(I3) 
+ ZBOS),IS = 1 , IL) 
216 FORMAT (IS5,15,4(T11,15,3(2X,D14.8) ,/)) 
17 - 18 
ENDIF 


CULL LLL KL KULEK SR Se Be d 
bah E E <`, PPR FRR PRO BBN AFR FR اک ا ` ` ` اک اک ا ا کک کک ا کے پک کے‎ PR اک‎ A کے ا‎ PO PR FN oF AN FR, FR RN ` Z. „Сл, a a AAN 
C CALCULATE THE TRUE TIMES AND THE TRUE TRAJECTORY 
C USE THIS CALL STATEMENT FOR MULTIPLE ARRAY TRACKING 

CALL TRJCS(KK,DATR,ZI,TD,XB,YB,ZB) 
C USE THIS CALL STATEMENT FOR SINGLE ARRAY TRACKING 
C CALL TRAJEC(KK,DATR,ZI,TD) Ἢ 

A14 = PHI (1,2) 

TRUX (KK) = TD(1) 

TRUY (KK) TD (2) 

TRUZ (KK) ΤΟ (5) 


C 
WRITE(4,306) KK,TRUX (KK) , TRUY (KKO , TRUZ (KK) 
(CN PIN Ph PS ANN ONIN ONIN ON ON EN ON EN NON ONS ON OS OP ον Da DUM ο S oR ee 
CHSSSSSHSSSSSSHESSSSSSSSSSSLSSSSSSSSSSSSSSSSSSSSSSSLSSHSSHESS 
E ROW OF H — MATRIX 
MINE = 1 
163 DO 132 IROW = 1 , IL 
NZDIFF = 4 
C USE THIS CALL STATEMENT TO RUN NOISE SUBROUTINE 
CALL NOISE (WINIT,WHTN) 
WHTN = ( 1.0 / 3.0 ) * WHTN 
C ZERO NOISE 
C WHTN = 0.0 
C USE THIS CALL STATEMENT FOR SINGLE ARRAY TRACKING 
C CALL CHROUW(IROMW,XKKM1,HROUW) 
C USE THIS CALL STATEMENT FOR MULTIPLE ARRAY TRACKING 
CALL CHROWS(IROU,XKKM1,HROW,XB,YB,ZB) 
C HFH HE HFF HF HF 8444 4Η 4444444444 4484844 ΒΗΒΗΒΗΦ4ΜΗΒΗΒΗΒΗ ΜΗ ΒΗΜ4 
Сб6 СК 1 = <Р С К / К- 1 1(5х5) * HT(Sx1) 2 / € H(1x5) 
[5 * Р [ K / K - 1 1(5x%5) * HT(Sx1) -* COVE V 10) 2 
CALL MMULT (PKKM1,HROW, IM, IM,GNUM) 
CALL VMULT (HROW,GNUM, IM,GDTEMP) 


20 


GDENOM = GDTEMP -« COVV(IROMW, IROW) 
DO 134 IX = 1 , IM 
GI(IX) = GNUM(IX) / GDENOM 
134 CONTINUE 
CHF 4F HF HF FHF HF HFF HF HF HF HF HF 4F 4F HF 4F 4F HF HF HF 44 4F 4F 4 4F 4F 44 44 HF 4 4 4F 3F HF 4F 4 44 HF 4F 4 4F 3F 44 4 4F 4 44 4 4 44 4 4 4 44 
Ceeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeceeeeeeeeeee 
C P ЕК / К 1 = {£ I(Sx5) - G C K 1(5«1) * H (1x5) } 
СЄ * РЕК / К —-1 1 
DO 135 IP = 1 , IM 
DO 136 JP = 1 , IM 


PDUM(IP,JP) - ( -1. * GI(IP)) * HROW(JP) 
IF(IP.EQ.JP) PDUM(IP,JP) = 1. + PDUM(IP ,JP) 
136 CONTINUE ! 
135 CONTINUE 


CALL PROD(PDUM,PKKM1,IM,IM,IM,PI) 
Ceeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeceeeeeeeceee 
C CALCULATE ΤΗΕ PREDICTION OF MEASUREMENTS 
C USE THIS CALL STATEMENT FOR SINGLE ARRAY TRACKING 
С CALL CZHAT (IROWM,XKKM1,ZHAT) 

C USE THIS CALL STATEMENT FOR MULTIPLE ARRAY TRACKING 

CALL CZHATS(IROW,XKKM1,ZHAT,XB, YB, ZB) 

ZIC(IROW) = ZIC(IROW) + WHTN * 0.00001 

ZDIFF(IROW) = ZIC(IROW) -— ZHAT 
C THREE SIGMA GATE 

P=DMAX1 (DABS(PI(1,1)),DABS(PI(3,3)) ,DABS(PI(5,5))) 

SIG=DSGRT ( (P/ ( (4860. ) ##2) ) + (DABS (COVV (IROW, IROW)))) 

GATE = 3.0 * SIG 


È 
IF(KK.LE.4) GO TO 149 
IF (DABS (ZDIFF (IROW)) .LT. GATE) GO TO 149 
(С 
WRITE (*,147) KK, IROW,GATE 
137 FORMAT(//,10X,' THREE SIGMA GATE HAS BEEN EXCEEDED’ 
- ,' AT TIME '4I4,' IN ROW ',I2,' GATE : ',D14.8) 
DO 148 LGJ * 1 , IM 
GI(LGJ) = 0.0 
148 CONTINUE 
C TAG INVALID TIME MEASUREMENT 
ZDIFF(IROW) = 999. 
C 
et 
CX [K / ως Με κκ yet GC KR 1 * «rk 
È -ZEK7/K-1 1X 
149 DO 15SO I - 1 , IM 
XI(I) 2 XKKM1(I) + GI(I)  ZDIFF(IROUD 
150 CONTINUE 
G ο с ш CCE ee oe LE LE 


IF(IROW.EQ.4) GO TO 152 
DO 153 I - 1 , IM К 
XKKM1(I) = XI(I) 
155 CONTINUE 
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DO 155 I 5 1 , IM 
DO 154 J - 1 , IM 
PKKMI(I,J) = PI(I,J) 


154 CONTINUE 
155 CONTINUE 
122 CONTINUE 


ἘΣ ΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣ 
152 DO 156 I = 1, IM 
XKK(I) = XI(I) 
XKKM1(I) = XI (1) 
C USE THIS ADDITIONAL STATEMENT FOR SMOOTHING 
XPO(I,KK) = XI(I) 
С 
pO 157 J = í , IM 
PKK(I,J) s PI(I,J) 
PKKM1(I,J) = PI(I,J) 
C USE THIS ADDITIONAL STATEMENT FOR SMOOTHING 
PS(KK,I,J) = PI(I,J) 
С 
157 CONTINUE 
156 CONTINUE 
(< ا‎ ΣΣ: 
C PREDICTION OF MEASUREMENTS BASED O 
DO 158 I = 1 , IL 
C EDIT INVALID TIME MEASUREMENTS 
IF (ZDIFF(1).GE.999.) GO TO 159 
C USE THIS CALL STATEMENT FOR SINGLE ARRAY TRACKING 
6 CALL CZHAT(I,XKKM1,ZHAT) 
C USE THIS CALL STATEMENT FOR MULTIPLE ARRAY TRACKING 
CALL CZHATS(I,XKKM1,ZHAT,XB,YB,ZB) 
ZDIFF(I) = DABS(ZIC(I) — ZHAT) 
GO TO 158 
159 ZDIFF(I) = 0. 
NZDIFF = NZDIFF -i 
158 CONTINUE 
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C ABSOLUTE AVERAGE VALUE OF THE DIFFERENCES IN MEASUREMENT 
IF (NZDIFF.EQ.0) GO TO 160 
ZDIFTO = DABS(ZDIFF(1)+ZDIFF (2) +ZDIFF (3) +ZDIFF (4) ) 
ZDIFAV = ZDIFTO / NZDIFF 
IF (KK.LE.4) GO TO 160 
IF (MINE.EQ.1) SMTH(KK) = ZDIFAV 
IF (MINE.GT.3) GO TO 160 
C USE THIS CONSTANT (2.0D-5) GATE 
IF(ZDIFAV.LT.2.0D-6) GO TO 160 
С 
WRITE(*,1472) KK 
1473  FORMAT(//,10X,' CONSTANT GATE HAS BEEN EXCEEDED AT' 
+ ,' TIME '.:25 
CCCA υ---ιοι 
C USE THESE STATEMENT TO INCREASE THE GAIN IN MULTIPLE % 


C SINGLE ARRAY TRACKING 


SIGAAC = 3.0 * SIGACC 
SIGDDI = 3.0 * SIGDIV 
SIGCCC = 3.0 * SIDBCC 


Σ 
C USE THIS CALL STATEMENT TO CALCULATE ADAPTIVE Q-MATRIX 
CALL OFIND(KK,XKK,PKK,SIGAAC,SIGDDI,SIGCCC,A14,@) 
CALL ADD(PKK,Q,IM,IM,PKKM1) 
MINE = MINE + 1 
GO TO 143 
COCE 
160 MINE = 1 
NZDIFF = 4 


WRITE(7,3O1) KK,(XKK(J),J = 1 , IM) 
WRITE(12,501) KK,(PKK(I,I),I 2 1 , IM) 
301  FORMAT(IS,S(A4X,D14.8)) 


C 
XKERR (KK) = XKK C1) — TRUX (KK) 
YKERR (KK) = XKK(3) - TRUY (KK) 
ZKERR (KK) = XKK(S) - TRUZ (KK) 
C 


WRITE(9,306) KK,XKERR (KK) , YRERR (KK) , ZKERR (KK) 
3506 FORMAT (I3S,5(4X,D14.8)) 
C DETERMINE MAX & MIN ERRORS AND THE TIME SLOTS 
fein. CG. i) TREN 


KX1K = KK : 
KX2K = Kk 

KY1K = KK 

KY2K = KK 

KZ1K = KK 

KZ2K = KK 

XIKERR = XKERR(KK) 
X2KERR = XKERR (KK) 
Y1KERR = YKERR (KK) 
Y2KERR = YKERR (KK) 
LIKERR = ZKERR (KK) 


Z2KERR = ZKERR (KK) 

ENDIF 

IF (XKERR (KK) .GT.XIKERR) KXIK = KK 

IF (XKERR(IGOKKO . GT. X1KERRO) X1KERR 2 XKERR (KK) 
IF (XKERR(KK).LT.X2KERR) KX2K = KK 

IF (XKERR(KK).LT.X2KERR) X2KERR = XKERR (KE) 
IF (YRERR (KK) .GT.YIKERR) KYIK = KK 

IF (CYERERR (KK) .GT.YIKERR) YIKERR = YKERR (KK) 
IF (YEERR (KE) .LT.Y2KERR) КҮЗЕ = КК 

IF (YKERR (KK) .LT.Y2ZKERR) Y2KERR = YKERR (KK) 
IF (ZKERR(KK).GT.ZIKERR) KZ1IK = KK 

IF (ZKERR (KK) .GT.ZiKERR) ZIKERR = ZKERR(KK) 
IP (ZKERR(KK).LT.Z2KERR) KZ2E = KK 

1F (ZKERREKK).LT. ZZKERR) ZZKERR = ZIKERR (KK) 


C)))))))))))))))))))))))))))))))))55)5))5)D5DD ] D] CDD 1D 
C P [ K + 1 / K J=C€CPHI(SxS) * P CK / K J(Sx8) * PHIT(S»5) } 
C +Q CK 3 
C USE THIS CALL STATEMENT TO CALCULATE ADAPTIVE Q-MATRIX 
CALL GFIND(KK,XKK,PKK,SIGACC,SIGDIV,SIGCC,A14,Q) 
CALL PROD(PHI,PKK,IM,IM,IM,PHIPKK) 
CALL PROD (PHIPKK,PHIT,IM,IM,IM,PKTEMP) 
CALL ADD(PKTEMP ,Q,IM,IM,PKKM1) 
ë 
CALL MMULT (PHI,XKK,IM,IM,XKKM1) 
ΟΕ ο... 
C USE THESE STATEMENTS FOR SMOOTHING 
DO 302 IG = 1, IM 
XP(IG,KK) = XKK(IG) 
302 CONTINUE 
DO 303 III = 1 , IM 
DO 304 JJJ = 1 , IM 


SSi (KK, III, JJJ) = PKKMI(III, JJJ) 
PIKK, III, JJJ) = PKK(III,JJJ) 
304 CONTINUE 


303 CONTINUE 
C33 H3 HF HF HF HF E HF 4F FF HF HF HFF HF HF E HF E HF E HE HE HE HF E HF E HF HF HF HF 4F HF 4F E HF 4F HF HF HF HF HF HEF FF HEEE EH HF 
C SMOOTHING STARTS HERE 
IF(KK.LE.JTIME) GO TO 128 
DO SOO K s 1 , JTIME 
KI - JTIME — K + 1 
WRITE(*,561) KI 


S61 FORMAT(/,10X,'IN SMOOTHING AT TIME :',I5) 
DO 501 I = 1 , IM : 
ХР1(1) = XP6(I,KI) 
ΞΟΙ CONTINUE 
DO S02 I = 1 , IM 
DO 503 J = 1 , IM 
P2(I,J) = PS(KI,I,J) 
SS3(I,J) 2 SSi(KI,I,J) 


IF(KI.LE.4) GO TO SOS 
IF(SMTH(KI).GE.2.0D-6) SSS(I,J) = 3.6 * SSS(I,J) 
503 CONTINUE 


302 CONTINUE 
C € 9€ J€ € € JC 9€ J€ € 3€ 3€ J€ J€ Je ЗЕ ЗЕ J€ J€ Je 3€ 3€ 3€ 3€ J€ J€ 3€ 3€ Je Je 3 3€ 3€ JE TE 36 9€ JE AE AE 9€ 9€ Je 3€ 3€ 9€ Je Je 3€ 9€ 9€ 3 9€ 9€ 9€ 36 9€ 96 3 
C ACK) = PCK/K) * TRANSPOSECPHII * INVEP(K+1/K) J 


CALL TRANS (PHI,IM,IM,PHIT) 
CALL RECIP(SS3,IM,SS3R) 
CALL PROD(SS3,SS3SR,IM,IM,IM,CH) 
CALL PROD(PHIT,SSSR,IM,IM, IM, TEMP1) 
CALL PROD(P2,TEMP1,IM,IM,IM,AK) 
Q X 3€ 3e 3€ 3e 3€ 3€ 3€ 9€ JC 3€ 24e 96 3€ 3€ 3€ 3€ 3C 9€ 36 3€ 3€ 3€ 9€ 9€ 9C 9 9€ 96 96 3€ 9€ 9 چڊ 9€ چڊ چڊ جد 3€ 9 96 چڊ چڊ چڊ جد جد چڊ چڊ چڊ چڊ چڊ چڊ د جد چڊ چڊ‎ 96€ 


C X(K/N) 25 X(K/K) * A(K) * E X(K*1/ND) —- X(K+1/K) 1 
DO 504 I 5 1 , IM 
XNNM1(I) 2 XP(I,KI-*1) 
504 CONTINUE 
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CALL MMULT(PHI,XP1,IM,IM,SS2) 
CALL SUB(XNNM1,SS2,IM,1,TEMP2) 
CALL PROD(AK,TEMP2,IM,IM,1,TEMPZ) 
CALL ADD(XP1,TEMPS,IM,1,XKKS) 
DO SOS I 2 1 , IM 
ΧΕΙ. ΚΙ) 2 XKKS(I) 
Sos CONTINUE 
C$5$$9255$555555555555559155554555555555555£5$5555555555555 $5555 


WRITE(8,301) KI,(XKKS(J),J = 1 , IM) ` 
XSERR(KI) = XKKS(1) —- TRUX(KI) 
YSERR(KI) = XKKS(3) — TRUY (KI) 
ZSERR(KI) 2 XKKS(S) - TRUZ(KI) 


WRITEI(10O,3506) KI,XSERR (KI) , YSERR (KI) ,ZSERR (GEI) 
C DETERMINE MAX & MIN ERRORS AND THE TIME SLOTS 

IF (K.EQ0.1) THEN 

KX1S 
KX2S 
KY1S 
25S 
KZ1S9 
K 229 
Xi SERR 
XZSERR 
Y1SERR 
Y2SERR 
Z 1SERR 
Z2SERR 

ENDIF 
IF (XSERR (KI).GT.X1SERR) 


KI 


XSERR (KI) 
XSERR (KI) 
YSERR (KI) 
YSERR (KI) 
ZSERR (KI) 
ZSERR (KI) 


KX1S = KI 


IF(XSERR(KI).GT.X1SERR) 
IF (XSERR (KI).LT. X2SERR) 
IF CXSERR KI) LT. X2ZSERR) 
IF (YSERR CKI) . GT. YI SERR) 
kE (YSERR (KI) .GT.Y1SERR) 
IF CYSERRXKI).LT.Y2SERR) 
IF (YŠSERR (KI) .LT. Y2SERR) 
IF(ZSERR(KI).GT.Z1SERR) 
ἵπ z5SERR(KI).GT.Z1SERR) 


X1SERR = XSERR(KI) 


KX2S = 
X2SERR 
KY1S = 
Y1SERR 
KY2S = 
Y2SERR 
KZ1S = 
Z1SERR 


= XSERR (KI ) 
KI 
= YSERR(KI) 
KI 
= YSERR(KI) 
KI 
= ZSERR(KI) 


IP (ZSERR(KI).LT. Z2ZSERR) KZ2S = KI 
IF (ZSERR(KI).LT.Z2SERR) Z2SERR = ZSERR(KI) 


Oe 9€ J€ 3€ J€ 3€ J€ Je 3 J€ J& Je 9€ Je Je 9€ 3€ Je He 96 9 J€ 9€ J€ 3€ J€ Je 9e 9€ 9€ 9€ 3 9€ 3€ J€ 3€ 9€ 9e 3 9€ 9€ 90 9€ 9€ 9€ 96 9€ 90 360 ++ 


C P(K/N) 


0 7 
206 


= P (K/K)+A (K) *EP (K+1 /N) —P (K+1 /K) 1*TRANSPOSECLA (F) 1 
DO 506 I = 1 , IM 
DO 507 J = 1 , IM 
PNNMi (I,J) = Pi(KI+1,I,J) 
CONTINUE 
CONTINUE 
CALL SUB(PNNM1,SS3,IM,IM,TEMP4) 
CALL TRANS (AK , IM, IM, AKT) 
CALL PROD (TEMP4 , AKT, IM, IM, IM, TEMPS) 
CALL PROD (AK , TEMPS, IM, IM, IM, TEMP6) 


m 


CALL ADD(P2,TEMP6,IM,IM,PKKS) 
pO 508 I = 1, IM 
DD SO9 J = 1, IM 


Pi(KI,I,J) = PKKS(I,J) 
509 CONTINUE 
508 CONTINUE 


CSESSSSHSHSSSSSSSHSHSSSSSSSSLSSHSSSSSSSSSSSSLHSRSSSSSSSSSSH HSS 
WRITE (11,301) KI,(PEKS(I,I),I = 1 , IM) 
(Z3€3€ 3€ 3€ 3€ 9€ 3€ 3€ 3€ 9€ J& 9€ 9 9€ 96 9€ 9€ 9€ 3€ 3€ 9€ 9€ 9€ 3€ 9€ 3€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 39€ 39€ 3€ 9€ 9€ 9€ 39€ 39€ 9€ 9€ 9€ 9€ 9€ 96 9€ 3€ 
500 CONTINUE 
CHHHHHHHHHHHHHHHEHHHHHHHEHHHHSHHEHHEHHEHHHHHHHE HEHEHE HEHEHE EH 
128 CONTINUE 
WRITE (12,800) 
800 FORMAT(10X,' TIME',4X,' MAX. ERROR  ',4X,' TIME',4X 
+ , MIN. ERROR 7) 
WRITE(12,801) KX1K,X1KERR,KX2K,X2KERR,KY1K,Y1KERR, 
-KY2K,Y2KERR,KZ1K,ZiKERR,KZ2K,Z2KERR 
WRITE(12,801) KX1S,X1SERR,KX2S,X2SERR,KY1S,Y1SERR, 
+KY2S, Y2SERR ,KZ1S,Z1SERR,KZ2S , Z2SERR 
801 FORMAT(S(/,10X,I15,4X,D14.8,4X%,15,4X,D14.8)) 


STOP 
END 
C 
SUBROUTINE TRANS (AAQ,NR,NC, BB) 
REAL*@ AA(NR,NC) ,BB (NC ,NR) 
po 3 I = 1 , NR 
DO 30 J3 = 1 , NC 
BB(J,I) = AACI,J) 
5ο CONTINUE 
z CONTINUE 
RETURN 
END 
С 
SUBROUTINE PROD (AA,BB,NRA,NCA,NCB,CC) 
REAL*8 AA(NRA,NCA) ,BB(NCA,NCB) ,CC (NRA,NCB) 
DO 4 I 1 =, NRA 
DO 40 J = 1 =, NCB 
CC(I,J) = 0.0 
40 CONTINUE 
4 CONTINUE 
DO 41 I = 1 =, NRA 
DO 410 J = 1 , NCB 
DO 411 K = 1 , NCA 
CECI,J).- CCCI, JI k AACO eE DE 
411 CONTINUE 


410 CONT INUE 

41 CONT INUE 
RETURN 
END 


SUBROUTINE TRAJEC(KK,DATR,ZI,TD) 


96 


REAL*8 DATR(17),ZI(4),TD(3) , COEFF , RANGE , VEL , T 
DATA VEL/4860.0/,1IK/3/,1IM/5/ 


T = 0.0 


COEFF = 1.0 / VEL 
Z1I (1) =COEFF#DSORT ( ({(DATR (1) +10. OQ) *3€2) 


+ 


+( (DAPTR(2)+1S.O)**2)+((DATR(3)+1S.O)*w*2) ) 


Z1 (2) =COEFF*#DSORT (((DATR(1)—-15.0) #2) 


+ 


+( (DATR(2)+15.O)**2)+((DATR(3)+15.O)**2)) 


L1 (5) =COEFF*#DSQRT (( (DATR(1)+15.0) ##2) 


+ 


+ (Chee (2 ο ο eZ) + ( (DATK(S) +15. O) #2) ) 


Z1 (4) =COEFF*DSORT (( (DATR(1)+15.0) #¥2) 


+ 
DO 5 I 
POCI) 
5 CONTINUE 


+( (DATR(2)+15.0) #¥2) + ( (DATR(3) -15.0) ##2) ) 
κ τς 
DATR (1) 


C USE THIS STATEMENT FOR STRAIGHT RUN 
cC IFC (KK.LE.DATR(17)). AND. (KK.GT.DATR(16))) GO TO 50 
C USE THESE STATEMENTS FOR MANEUVERING RUN 
38 πα ιΕ ο αν. (hk oem 22)) GO TO 5Ο 
IF({KK.LE. 98).AND. (KK.GT.71)) GO TD SO 
IJ (C (Cr EQ SO) R. (KK EU. 990m) ΜΈΝ 


=з DATR (12) 
DATR(7) = 
DATR(8) = 


C 
C FIRST DATA FOR TRUE TRAJECTORY IN SINGLE ARRAY TRACKING 
C DATR(2) = 1300.0 
C DATR(OS) = 0.0 
C 
C SECOND DATA FOR TRUE TRAJECTORY IN SINGLE ARRAY TACKING 
DATR(2) = 1000.0 
DATR(3) = 300.0 
DATR (4) = -30.0 
DATR(S) = 0.0 
DATR(AÓ) = 0.9 
DATR(7) = 0.0 
DATR(8) = 0.0 
DATR(9) = 0.0 
DATR(10) = 9.0 
DATR(11) = 0.0 
DATR(12) = 4.712389 
DATR(13) = 0.1745329 
DATR(14) 20.1 
DATR (15) = 8.1 
ENDIF 
= 
=l DATR(7) = 0.9 
DATR(8) = 0.0 
DATR(14) = 1.351 
ΕΠ ΤΟ 51 
30 DATR(14) = 0.005 


= DATR(12) + DATR(1S) * DATR(14) 
DATR (15) * DCOS(DATR (12) ) 
DATR(15) * DSIN(DATR((12) ) 
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S1 DO S2 I s 1 , IIM 
DATR(I) = DATR(I) + DATR(I+3) * DATR(14) 
+ + (((DATR(14))##2)/2) * DATR (1+6) 
52 CONTINUE 
T 2, T * DATR(14) 
IF (DABS(T - 1.31).LE.0.0001) RETURN 
GO To 53 
END 


SUBROUTINE TRUJCS(KK,DATR,ZI,TD,XB,YB, ZB) 
REAL*8 DATR(17) ,Z1¢4) ,TD(3) ,XB(4) ,YB(4) ,ZB(4) , COEFF 
REAL*8 VEL,T 
DATA VEL/4860.0/,IIK/3/,IIL/4/,1IM/S/ 
το 
COEFF = 1.0 / VEL 
DO 12 I = 1 , TIL | 
ZI(I) = COEFF * DSORT(((DATR(1) — XB(I))*+*2) 
+  * ((DATR(2) — YBODO **2) * ((ODATROD. - ZBODO **2)) 
12 CONTINUE 
DG 120 I1 =i ir 
TD(I) = DATR(I) 
120 CONTINUE 
C USE THIS STATEMENT FOR STRAIGHT RUN 
IF ( (KK.LE. DATR(17)) AND. (KK.GT.DATR(16))) GO TO 121 
C USE THESE STATEMENTS FOR MANEUVERING RUN 
ο 128  IF((KK.LE.49) . AND. (KK. GT. 222) GO TO 121 
IF((GK.LE. 98) . AND. (KK. GT. 71)) GO TO 121 
IF((KK. EGO. 5S0) . OR. (KK. EQ. 99) ). THEN 
DATR(2) 7000.0 
DATR (3) 
DATR (4) 
DATR (5) 
DATR (4) 
DATR (7) 
DATR (9) 
DATR (9) 
DATR (10) 
DATR (11) 
DATR(12) 
DATR (13) 
DATR (14) 
DATR(15) 
ENDIF 


0000000000000000001D0 


127 DATR(7) = O. 
DATR (8) = O. 
DATR(14) = 1.31 
σα το αοὁ 

121 DATR(14) = 90.005 

124 DATR(12) = DATR(12) + DATR(13) * DATR(14) 
DATR(7) = DATR(15) * DCOS(DATR(12) ) 


O 
О 
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DATR(8) = DATR(1S) * DSIN(DATR(12) ) 
pez ΓΡ ος 1 Ive tam 
DATR (1I) DATR(I) + DATR(I+3) * DATR(14) 
Ἔ + (((DATR(14) )##2)/2) * ὈΑΤΗ͂(Ι 1-6) 
125 CONTINUE 
T = T + DATR(14) 
IFeDABS IT = 1.51).LE.O.O001) RETURN 
GO TO 124 
END 


SUBROUTINE CHROW (IROW, XKKM1 ,HROW) 
~REAL*8 HROW(S) ,XKKM1(5) ,COEFF ,DENOM, DENOM1 , DENOM2 
REAL*8 VEL,A1,A82,A3,DENOMS , DENOM4 

DATA VEL/4860. 0/ 

COEFF = 1.0 / VEL | 

DENOM1=DSGQRT ( ( (XKKM1 (1) +15.0) **2)+ ( (XKKM1 (S) +15. O) **2) 


+ +( (XKKM1 (5) +15.0) *#2) ) 

DENOM2=DSOQRT ( ¢ (XKKM1 (61) -15.0) ¥##2) +((XEKM1 (3S) +15. 0) *#*2) 
+ +( (XKKM1 (5) +15.0) *#2) ) 

DENOMS3ZDSQRT ( C CXKKM1 C10 15.0) **2) -COXKKM1 C3) 215.0) * *2) 
- +( (XKKM1 (S) +15. 0) x*2) ) 

DENOM4=DSORT ( ( (XKKM1 (1) +15. O) ##2) +( (XKKM1 (3) +15.0) ##2) 
+ *OXOXKIKMI (CO) 715. 0) **2)) 

AL = 1.90 

AZ = 1.90 

AS = 1.90 


DENOM = DENOM1 


IF (TROW.EG.2) DENOM = DENOM2 b 

IF (TROW.E@.3) DENOM = DENOMS 

IF (TROW.EQ.4) DENOM = DENOM4 

IF (TROW.EQ.2) AL = - 1.0 

HROW(1) = COEFF * ((XKKM1(1) + AL * 15.0) / DENOM) 
IF(IROW.EQ.35) AZ = — 1.0 

HROW(S) = COEFF * ((XKKM1(3) + AZ * 15.0) / DENOM) 
IF (TROW.EQO.4) AS = - 1.90 

HROW(S) = COEFF * ((XKKM1(5) + AS * 15.0) / DENOM) 
HROW(2) = 0.0 

HROW(4) = 0.0 

RE TURN 

END 


SUBROUTINE CHROWS (IROW, XKKM1,HROW,XB,YB, ZB) | 
REAL*8 HROW (5) ,XKKM1 (5) , COEFF , DENOM , VEL , XB (4) ,YB (4) 
REAL*8 X0,YO,Z0,ZB(4) 

DATA VEL/4860.0/ 

COEFF = 1.0 / VEL 

XO = XB(IROW) 


YO = YB(IROW) 

ZO = ZBC(TROW) 

DENOM = DSORT ( ( (XKKM1 (1) -XO) ##2) +( (XKKM1 (3) -YO) ##2) 
a + (CCAEEMI (S5) σου ες) 
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HROW(1) = COEFF «κ ((XKKM1«1) - XO) / DENOM) 
HRQW(2) = 0.0 

HROW(S) = COEFF * ((XKKM1L(3) - YO) / DENOM) 
HROW(4) = 0.0 

HROW(S) = COEFF * ((XKKM1«929) - ZO) / DENOM) 
RETURN 

END 


SUBROUTINE MMULT (AA , BB , NRA , NCA , CC) 

REAL#S AA (NRA, NCA) , BB (NCA) , CC (NRA) 

DO 6 I = 1 , ΝΒΑ 

CEI) = o 

DO 60 J = 
СС‹ї) = С 

CONTINUE 

CONTINUE 

RETURN 

END 


C Nu I 


9 

O - 

1 , NCA 
C(I) + AACI,J) * BBCI) 


SUBROUTINE VMULT(AA,BB,NE,CC) 
REAL*8 AA (NE) ,BB (NE) ,CC 
CC = 0.0 
DO 7 I = 1 =, NE 
CC = CC + AACI) * BBCI) 
CONTINUE 
RETURN 
END 
SUBROUTINE CZHAT(IROW, XKKM1 , ZHAT) 
REAL*S XKKM1(S) ,ZHAT, COEFF ,VEL 
DATA VEL/4860.0/ 
COEFF = 1.0 / VEL 
IF (IROW.EQ.1) ZHAT=COEFF*DSORT ( ( (XKKM1 (1) +15.0) #2) + 
+ ( (XKKM1 (Z) +15. O) **2) + ( (XKKM1 (S) 415.0) ##2) ) 
IF(IROW.EQ.2) ZHAT=CDEFF+*DSƏRT ( ( (XKKM1 (1) —15. O) **2) + 
+ ( (XKKM1 (3) +15. 0) 2) e CXKKM1 (5) *15.0) 2) ) 
IF(IROW.EQ.Z) ZHAT-COEFF*DSORT (((XKKM1 (1) -15. 0) 9*2) 4 
+ ( (XKKM1 (S) -15. 0) **+2) + ( (XKKM1 (S) 415.0) ##2) ) 
IF (IROW. EG. 4) ZHAT=COEFF#DSGRT (((XKKM1 (1) +15. O) ##2) + 
+ ((XKEM1 (3) +15. 0) **2)+( (XKKM1 (S) ~15. 0) ##2) ) 
RETURN 
END 


SUBROUTINE CZHATS(IROW,XKKM1,ZHAT,XB,YB,ZE) 

REAL*8 XKKM1(5),Z2HAT,COEFF ,VEL,XB(4) , YB(4) ,ZBCA) , XO 
REAL*8 YO,ZO 

DATA VEL/4860.0/ 

COEFF = 1.0 / VEL 


XO - XB(IROW) 
YO = YB«(IROW) 
ZO = ZBC(CTROW) 
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80 


110 
Τι 


ZHAT = COEFF «* DSGRTCCCXKKMT1€1) — XO)+**2) + 

ο ο eee) ee (κκ στ = Z093**2)) 
RETURN 
END 


SUBROUTINE NOISE(R,P) 

REAL*8 Y(6) ,X(6) ,S(5) ,R,P,BB,P1 

DATA Y/0.0,.0228,.0668,.1357,.2743,. 5/ 
DATA X/-3.01,-2.0,-1.5,-1.0,-0.6,0.0/ 
DATA S8/43.8596,11.3636,7.25689,2.891352,2. 565887/ 
BB - 1.0 

P1 = R * 317.0 

R = DMOD(P1 , BB) 

P = R 

I = 1 | 

IF(P.GT.0.5) P= 1.0 - R 
IF(P.LT.Y(I+1)) GO TO 80 

I = I + 1 

GO TO 8 

Pz((P-Y(OD) * SC(D * XODD 

IF(R. GE.0.5) P 2-P 

RETURN 

END 


H H IH 


SUBROUTINE ADD(AA,BB,NR,NC,CC) 
REAL«8 AA(NR,NC) , BB(NR,NC) ,CC(NR,NC) 
DO 9 I = 1 , NR 
BE 90 J = 1 , NC 
CC(I,J) = AA(I,J) + BB(I,J) 
CONT I NUE 
CONTINUE 
RETURN 
END 


SUBROUTINE GFIND(KK,XKK,PKK,SIGACC,SIGDIV,SIGCC,A,0) 
REAL*8 XKK(5) ,PKK(5,5) ,Q(5,5) ,SIGACC,SIGDIV,SIGCC,A 
REAL«8 A2,A3,5,C,D,E1,£12,E2,61,G62,G3,SIGAAC,SIGDDI 
REAL*8 SIGCCC,AÀ1 
INTEGER KK 
IF(KK.NE.1) GO TO 111 
posi 1 4 5 

DO 110J 2 1, 5 

ANI J) = 0.0 


CONTINUE 
CONTINUE 
SIGACC = SIGACC **2 
(515) = (SIGDIV **2) * (ñ **2) 


SIGCC = SIGCC **2 

G1 ( ñ **2 ) / 2.0 
G2 G1 **2 

G3 ^ * G1 


101 


113 
112 


120 
12 


“h 


140 
14 


143 


A2 = A **2 

А1 = XKK(2) **2 + XKK(4) **2 

A3 2 XKK(2) / DSORT (41) 

В = XKK(4) . 

C 2 XKK(4) / DSORT(A1) 

D = XKK (2) 

El = ( A3 **2) * SIGACC + ( B **2 ) * SIGCC 
E12 - A3 * C * SIGACC - B * D * SIGCC 

E2 = ( C **2 ) * SIGACC + ( D **2 ) * SIGCC 
Q(1,1) = El * G2 

Q(1,2) = GS * El 

Q(1,3) - E12 * G2 

Q(1,4) = GS * E12 

Q(2,2) = A2 * El 

Q(2,3) 2 G3 * E12 

Q(2,4) = A2 * E12 

Q(3,3) = G2 * E2 

Q(3,4) = GZ * E? 

Q(4,4) = A2 * E2 

DO 112 I=1, 4 


DO ITI e T 
0(1,1) = 0(2,1) 
CONTINUE 
CONTINUE 
RETURN 
END 


SUBROUTINE SUB(AA,BB,NR,NC,CC) 
REAL*8 AA(NR NC) ,BB(NR,NC) ,CC(NR,NC) 
DD 12 I = 1 , NR 
DO 120 J = 1, NC 
CE IIJ = AA(1.J) ¬ BBCI) 
CONTINUE 
CONTINUE 
RETURN 
END 


SUBROUTINE RECIP (AA,NN,CC) 
REAL#8 AA(NN,NN) ,DD(S,10) ,CC (NN,NN) 
DO 14 K = 1 =, NN 
DO 140 J = 1 , NN 
DD(K,J) = AACKJ) 
CONT INUE 
CONTINUE 
DO 141 K = 1 , NN 
I = K + NN 
DO 142 J=6, 10 
IF(I.NE.J) GO TO 143 
DD(K,J) = 1. 
GO TO 142 
DD(K,J) = 0. 


102 


142 
141 


147 


146 
144 


149 
148 


CONTINUE 


CONTINUE 
DO 144 K = 1 , NN 
M = K +1 
DO 145 J = M , 10 
DD(K,J) = DD(K,J) / DD(K,K) 
CONTINUE 


DD(K,K) - 1. 
DO 146 L - 1 , NN 
IF(L.EQ.K) GO TO 146 
DO 147 I - 1 , 10 
IF(I.EDQ.K) GO TO 147 
DD(L,I) = DD(L,I) - DD(L,K) * DD(K,I) 
CONTINUE | 
DD(L,K) = O.. 
CONTINUE 
CONTINUE 
DO 148 K = 1 , NN 
DO 149 J = 1 , NN 
I = J + NN 
δή ο). 590 Κω 
CONTINUE 
CONTINUE 
RETURN 
END 


APPENDIX C 
PLOTTING PROGRAM LISTING FOR HP PLOTTER 


SSTORAGE: 2 
DEBUG 
SNOLIST 
m 
PROGRAM PLOTTER 
5 
CHARACTER*40 TITLE 
CHARACTER*SS LEGEND,SUBTITLE 
CHARACTER*25 NAMEX,NAMEY 
REAL X (245) ,¥ (245) 
REAL 0(245) ,P(245) ,R(245) ,S(245) ,T(245) ,U(245) 
INTEGER*2 IC 
DATA IC/O/ 
C 
C USE THESE FOR MULTIPLE ARRAY TRACKING 
C TITLE s'MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN’ 
TITLE =' MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN’ 
È 
C USE THESE FOR SINGLE ARRAY TRACKING 
C TITLE =‘SINGLE ARRAY ADAPTIVE MANEUVERING RUN‘ 
C TITLE 2'SINGLE ARRAY ADAPTIVE STRAIGHT RUN’ 
OPEN(S,FILE-'XKK.DAT',STATUSS'OLD °’) 
DO 32 LENG = 1 , 241 
READ (5,*#,END=33) O(LENG) ,P (LENG) ,R‘(LENG) ,S‘(LENG) , 
+ T (LENG) ,U (LENG) 
E CONTINUE 
ID CONTINUE 
LENG - LENG - 1 
CLOSE (5,STATUS-2 ‘KEEP °) 
NAMEX -'XEtK/K1 (Ετ)΄’ 
NAMEY -'YCK/K1 (Ετ)΄’ 
SUBTITLE =’ i 
LEGEND ='FILTERED ESTIMATE OF TRAJECTORY’ 
CALL DRAWER (TITLE ,NAMEX , NAMEY , LEGEND ,P,S,LENG, 
+ SUBTITLE) 
OPEN(S,FILE2'PKK.DAT',STATUSS'OLD ') 
DO 34 LENG = 1 , 241 
READ(S,*,END-35S) Q(LENG) ,P (LENG) ,R(LENG) ,S(LENG) , 
I T (LENG) ,U (LENG) 
34 CONT INUE 
35 CONTINUE 


LENG = LENG - 1 

CLOSE (5,STATUS- ' KEEP °) 
NAMEX -'TIME SLOTS’ 
NAMEY =' (FT##*2) ° 
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40 
41 


+ 


+ 


+ 


+ 


-- 


+ 


+ 


"Ë 


SEBDITLE 2'PLK/K1(1,1) ' 
LEGEND sS'FILTERED ERROR COVARIANCE P(K/K)*' 
CALL DRAWER (TITLE ,NAMEX , NAMEY , LEGEND ,O,P ,LENG, 
SUBTITLE) 
NAMEY =’ ((FT/SEC) ##2) ' 
SUBTITLE =‘PCK/K1(2,2) ° 
CALL DRAWER (TITLE, NAMEX , NAMEY , LEGEND, O0,R,LENG, 
SUBTITLE) 
NAMEY =’ (FT##*2) ' 
SUBTITLE -'P[K/K1(3,3)' 
CALL DRAWER(TITLE,NAMEX ,NAMEY ,LEGEND,O,S,LENG, 
SUBTITLE) 
NAMEY s'((FT/SEC) **«2) ° 
SUBTITLE ='PIK/K1(4,4) ’ 
CALL DRAWER (TITLE,NAMEX ,NAMEY ,LEGEND,O,T,LENG, 
SUBTITLE) 
NAMEY =’ (FT##2) ° 
SUBTITLE ='Р{К/К1(5,5)'/ 
CALL DRAWER (TITLE , NAMEX , NAMEY , LEGEND, O, ÛU, LENG, 
SUBTITLE) 
OPEN (5,FILE=‘ XKERR. DAT’ ,STATUS=’OLD’) 
DO 38 LENG = 1, 241 
READ (5,#*,END=39) O(LENG) ,P (LENG) ,R(LENG) ,S(LENG) 
CONTINUE 
CONTINUE 
LENG = LENG - 1 
CLOSE (5,STATUS- 'KEEP ') 
NAMEY ='X ERROR (FT) ' 
SUBTITLE =’ | 
LEGEND = ERROR IN FILTERED ESTIMATE’ 
CALL DRAWER (TITLE , NAMEX , NAMEY , LEGEND , O0,P, LENG, 
SUBTITLE) 
NAMEY -'Y ERROR (FT) ' 
CALL DRAWER(TITLE,NAMEX ,NAMEY,LEGEND,O,R,LENG, 
SUBTITLE) 
NAMEY -2'Z ERROR (FT) ’ 
CALL DRAWER (TITLE,NAMEX ,NAMEY,LEGEND,O,S,LENG, 
SUBTITLE) 
OPEN (5,FILE=’XKN. DAT’ ,STATUS=’OLD’) 
DO 40 LENG = 1 , 241 
READ(S5,*,END241) O(LENG) ,P(LENG) ,R(LENG) , S(LENG) , 
T (LENG) ,U (LENG) 
CONT INUE 
CONTINUE 
LENG = LENG - 1 
CLOSE (S, STATUS= ‘KEEP ') 
NAMEX ='* X[K/N1 (FT) 
NAMEY ='YCK/N] (ЕТ) 
LEGEND =‘’SMOOTHED ESTIMATE OF TRAJECTORY ' 
CALL DRAWER (TITLE,NAMEX,NAMEY ,LEGEND,P,S,LENG, 
SUBTITLE) 


OPEN (S ,FILE= PKN.DAT ' ,STATUS-Z' 'QLD ') 
DO 42 LENG = 1 , 241 
READ(S,*,ENDZ243) O(LENG),P(LENG) ,R(LENG) , S(LENG) , 
+ ii T (LENG) ,U(LENG) 
42 CONT INUE 
43 CONTINUE 
LENG = LENG - 1 
CLOSE (S.,STATUS= “KEEP ° ) 
NAMEX =‘TIME SLOTS’ 
NAMEY =‘ (FT *#*2) ‘ 
SUBTITLE =‘PEK/NI (1,1) 
LEGEND =‘SMOOTHED ERROR COVARIANCE FPCK/N) ° 
CALL DRAWER (TITLE ,NAMEX , NGMEY ,LEGEND,O,P,LENG, 
+ : SUBTITLE) 
NAMEY =' ( (FT/SEC) **2) ' 
SUBTITLE =‘PCK/NI (2,2) ¢ 
CALL DRAWER ( TITLE ,NAMEX , NAMEY , LEGEND, O,R, LENG, 
+ SUBTITLE) 
NAMEY =' (FT##2) ° 
SUBTITLE ="PCK/NI(3,3) ° 
CALL DRAWER (TITLE,NAMEX ,NAMEY,LEGEND,O,S,LENG, 
+ SUBTITLE) 
NAMEY =‘ ( (FT/SEC) ##2) ° 
SUBTITLE =’PCK/N1 (4,4) ° 
CALL DRAWER (TITLE,NAMEX ,NAMEY,LEGEND,O,T,LENG, 
+ SUBTITLE) 
NAMEY =‘ (FT ##2) ° 
SUBTITLE, = PEK/NI(3S5a) ° 
CALL DRAMWER (TITLE ,NAMEX , NGMEY , LEGEND ,O,U,L ENG, 
+ SUBTITLE) 
OPEN (S,FILE='XSERR.DAT’ ,STATUS=’OLD‘) 
DO 44 LENG = 1 , 241 
READ (S,*, END=45) OC(LENG) ,P (LENG) ,R(LENG) ,S (LENG) 
44 CONT INUE 
45 CONTINUE 
LENG = LENG - 1 
CLOSE (S5,STATUSz ' KEEP ') 
NAMEY =‘X ERROR (FT) ° 
SUBTITLE =‘ : 
LEGEND =“ ERROR IN SMOOTHED ESTIMATE’ 
CALL DRAWER(TITLE,NAMEX , NAMEY , LEGEND ,Q,F ,LENG, 
* SUBTITLE) 
NAMEY =‘Y ERROR (σσ 
CALL DRAWER(TITLE,NAMEX,NAMEY,LEGEND,O,R,LENG, 
+ SUBTITLE) 
NAMEY ='Z ERROR (FT) “ 
CALL DRAWER (TITLE,NAMEX ,NAMEY,LEGEND,O,S,LENG, 
τ SUBTITLE) 
STOP 
END 
SUBROUTINE DRAWER (TITLE,NAMEX ,NAMEY,LEGEND,X,Y, 
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00000 


С) 


USE 


USE 


LENG, SUBTITLE) 


CHARACTER*4O TITLE 
CHARACTER*35 LEGEND , SUBTITLE 
CHARACTER*25 NAMEX,NAMEY 


REAL 


X (245) ,Y (245) 


INTEGER*2 IC 


DATA 


CALL 
CALL 
CALL 
CALL 
CALL 


THES 


IC/O/ 


ED 

CUP (1,0) 

PLOTS(0,9600,30) 

SYMBOL (2.0,6.65,. 20, TITLE,O0. 0,40) 
SYMBOL (2.0,6.25,.175,LEGEND,0. 0,25) 


FOR NOISELESS TRACKING 


Γκ SYMBOLU (6ee4,6.25,.175, WETHOUT NOISE’ ,0.0,13) 


THES 
CALL 


CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 


‘CALL 


CALL 


CALL 
CALL 


CALL 
CALL 
CALL 


FOR NOISY TRACKING 

SYMBOL (6.84,6.25,.175,’ WITH NOISE',0.0,12) 

SYMBOL (1.460,2.45,.20,SUBTITLE,90.0,35) 

PLOT (1.00,1.00,-3) 

PLOT(8.0,0.0,3) 

PLOT (8.0,6.0,2) 

PLOT (0.0,6.0,2) 

pusgqco s, 0.0, 2) 

PLOT(8.0,0.0,2) 

SCALE (X,6. 00 ,LENG, 1) 

SCALE (Y, S. O0, LENG, 1) 

STAXIS(.180,.20,.15,.112,-1) 

AXIS(1.5,1. 5,NAMEX,-13,6.00,00. ,X(LENG*1) , 
X (LENG*2) ) 

BEuBXIS(..15,.20,.111,.112,2) 

AXIS(1.5,1. 5,NAMEY,12,3.00,90. , Y(LENG*1) , 
Y (LENG*2) ) 

PLOT(1.50,1.50,—-23) 

LINE(X,Y,LENG,1,0,2) 

PLOT(0.0,0.0,999) 


RETURN 


END 


SUBROUTINE ED 
CHARACTER*1 C1,C2,C3,C4 
INTEGER*2 IC (4) 


EQUIVALENCE 


DATA 


(eine n ο τρ ο ος τοσο, 
(3 IC (35) 
IC/16#1B,16#5B,16#32,16#4A/ 


μπες 1 Cr, caves ,C4 
FORMAT (1X, 4A1) 

RETURN 

END 


SUBROUTINE CUP (N,M) 


107 


F) 


CHARACTERS] GI sn rS conte cs) 

CHARACTER*5 CBUFF 

INTEBER*2 IC (Q) 

EQUIVALENCE (E1,I1€(1)) , (G2, 1c (eco REN 
(C8, IC (4) ) , CCBUFF ,LC(1)) 

DATA IC/146#1B, 146#5B,16#3B,16#66/ 

L=10000+100*4N+M 

WRITE (CBUFF,2)L 

FORMAT (IS) 

WRITE(*,1) C1,C2,LC(2),LC(35) ,CS,LC (4) ,LC eS) ,C8 

FORMAT (1X,8A1,\) 

RETURN 

END 
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APPENDIX D 
PLOTTING PROGRAM LISTING FOR MONITOR 


STORAGE: 2 
SDEBUG 
*NOL IST 
fe 
PROGRAM MONITOR 
E 
CHARACTER*4O TITLE 
CHARACTER*35 LEGEND 
CHARACTER#25 NAMEX ,NAMEY 
REAL X(245) ,Y¥ (245) 
REAL 0(245) ,P (245) ,R(245) ,S (2245) ,T (245) ,U (245) 
INTEGER*2 IC 
DATA IC/O/ 
区 
C USE THESE FOR MULTIPLE ARRAY TRACKING 
Ë TITLE =' MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN’ 


TITLE =‘ MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN’ 


È 
C USE THESE FOR SINGLE ARRAY TRACKING 
È TITLE ='SINGLE ARRAY ADAPTIVE MANEUVERING RUN’ 
C TITLE ='’SINGLE ARRAY ADAPTIVE STRAIGHT RUN’ 
OPEN(S,FILE-'XKK.DAT' ,STATUSS 'OLD ') 
DO 32 LENG » 1 , 241 
READ(S,*,END-33) O(LENG) ,P (LENG) ,R(LENG) , S(LENG) , 


+ T (LENG) ,U (LENG) 
32 CONTINUE 
== CONTINUE 

LENG = LENG - 1 

CLOSE (5 ,STATUS= ‘KEEP °’) 

NAMEX ='X[K/K1 (FT) ° 

NAMEY =‘Y[IK/K1  (FTO* 

LEGEND =‘FILTERED ESTIMATE OF TRAJECTORY ' 

CALL DRAWER (TITLE,NAMEX ,NAMEY ,LEGEND ,P,S,LENG) 

OPEN (5,FILE='PKK. DAT’ ,STATUS='OLD’) 

DO 34 LENG = 1 , 241 

READ (S,*,END=35) O(LENG) ,P (LENG) ,R(LENG) ,S(LENG) , 

+ T (LENG) ,U (LENG) 
3 CONT INUE 
35 CONTINUE 


LENG = LENG - 1 

CLOSE (S ,STATUS= ‘KEEP °) 

NAMEX =‘TIME SLOTS’ 

MAMEY s'PCK/K1(1,1)^ 

LEGEND s'FILTERED ERROR COVARIANCE P(K/K)' 
CALL DRAWER(TITLE,NAMEX , NAMEY , LEGEND ,O,P , L ENG) 
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38 


40 
41 


42 


43 


NAMEY =‘PCK/K1(2,2) ‘ 

CALL DRAWER(TITLE,NAMEX , NAMEY , LEGEND , O,R,LENG) 

NAMEY ='PCK/K1(3,3) ‘ 

CALL DRAWER (TITLE,NAMEX , NAMEY,LEGEND,O,S,LENG) 

NAMEY =‘PCK/K1(4,4) ' 

CALL DRAWER (TITLE,NAMEX ,NAMEY,LEGEND ,O,T,LENG) 

NAMEY =‘PLK/K1(5,5 

CALL DRAWER (TITLE, NAMEX ,NAMEY ,_LEGEND,O,U,LENG) 

OPEN (5,FILE=’ XKERR. DAT’ ,STATUS=‘OLD’) 

DO 38 LENG =1 , 241 

READ (S,*,END=39) O(LENG) ,P(LENG) ,R(LENG) ,S (LENG) 

CONTINUE 

CONTINUE 

LENG = LENG - 1. 

CLOSE (5,STATUS=‘KEEP ’) 

NAMEY ='X ERROR (FT) ’ 

LEGEND =‘ERROR IN FILTERED ESTIMATE’ 

CALL DRAWER (TITLE,NAMEX,NAMEY , LEGEND ,O,P,LENG) 

NAMEY =‘Y ERROR (FT) ' 

CALL DRAWER (TITLE ,NAMEX , NAMEY , LEGEND ,O,R,LENG) 

NAMEY ='Z ERROR (FT) ' 

CALL DRAWER (TITLE ,NAMEX , NAMEY , LEGEND ,O0,S,LENG, 

OFEN (S,FILE=' XKN. DAT ' ,STATUS= ' OLD ') 

DO 40 LENG = 1 , 241 

READ (5,*,END=41) O (LENG) ,P (LENG) ,R (LENG) ,S (LENG) , 
T (LENG) ,U(LENG) 

CONTINUE 

CONTINUE : 

LENG = LENG - 1 

CLOSE (5,STATUS= ‘KEEP’ ) 

NAMEX =‘XCK/NI (FT) 

NAMEY =‘YCK/NI (FT) ° 

LEGEND =‘SMOOTHED ESTIMATE OF TRAJECTORY ' 

CALL DRAWER (TITLE , NAMEX , NAMEY , LEGEND ,P,S, LENG) 

OPEN (S,FILE= PKN.DAT  ,STATUS= ° DLD °) 

DO 42 LENG = 1 , 241 

READ (5,*,END=43) O(LENG) ,P (LENG) ,R (LENG) ,S (LENG) , 
T (LENG) ,U (LENG) 

CONTINUE 

CONTINUE 

LENG - LENG - 1 

CLOSE (5, STATUS= ‘KEEP ') 

NAMEX =‘TIME SLOTS’ 

NAMEY =‘'PCK/NI (1,1) ’ 

LEGEND =‘SMOOTHED ERROR COVARIANCE P(K/N) ' 

CALL DRAWER (TITLE,NAMEX , NAMEY , LEGEND ,O0,P , LENG) 

NAMEY -2'PLK/N1(2,2)' 

CALL DRAWER (TITLE, NAMEX ,NAMEY ,LEGEND,O,R,LENG) 

NAMEY ='PCK/NI(3,3) ° 

CALL DRAWER(TITLE,NAMEX , NAMEY , LEGEND ,O,S,LENG) 

NAMEY -'PCK/N1(4,4)* 


ωωωωοω 


O 


CALL DRAWER (TITLE ,NAMEX , NAMEY , LEGEND ,O,T, LENG) 
NAMEY =‘PEK/N1 (5,5) 

CALL DRAWER (TITLE ,NAMEX , NAMEY , LEGEND ,O,U,LENG) 
OPEN (5,FILE=‘XSERR. DAT’ ,STATUS=‘OLD’) 

DO 44 LENG = 1 , 241 | 

READ (S,*,END=45) D(LEMG) ,P (LENG) ,R (LENG) ,S (LENG) 
CONTINUE 

CONTINUE 

LENG = LENG - 1 

CLOSE (5, STATUS= ‘KEEP ° ) 

NAMEY =‘X ERROR (FT) ° 

LEGEND =’ERROR IN SMOOTHED ESTIMATE’ 

CALL DRAWER (TITLE,NAMEX,NAMEY,LEGEND,O,P,LENG) 
NAMEY =‘Y ERROR (FT) ' ! 

CALL DRAWER (TITLE,NAMEX,NAMEY,LEGEND,O,R,LENG) 
NAMEY =‘Z ERROR (FT) ‘ 

CALL DRAWER (TITLE,NAMEX,NAMEY,LEGEND,O,S,LENG) 
STOP 

END 

SUBROUTINE DRAWER (TITLE ,NAMEX , NAMEY , LEGEND , X,Y ,LENG) 
CHARACTER*40 TITLE 

CHARACTER*35 LEGEND 

CHARACTER*25 NAMEX ,NAMEY 

REAL X(245),Y(245) 

INTEGER*2 IC 

DATA IC/O/ 


CALL ED 

CALL CUP(1,0) 

CALL PLOTS(0,99,99) 

CALL SYMBOL (0.5,5.15,.20,TITLE,0.0,40) 
CALL SYMBOL (1.04,4.75,.175,LEGEND,0.0,35) 


USE THIS FOR NOISELESS TRACKING 
CALL SYMBOL (5.38,4.75,.175, ‘WITHOUT NOISE’ ,0.0,15) 


USE THIS FOR NOISY TRACKING 
Geet Ss meee (S.58,4.75,-175, WITH NOISE’ ,0O.0,13) 


CALL PLOT(1.00,1.00,-3) 
CALL SCALE (X , 64. 00 ,LENG , 1) 

CALL SCALE (Y,3.00,LENG , 1) 

Ger mesmaxis(.190,.20,.15,.112,-1) 

CALL AXIS (O. ,O. ,NAMEX,-13,6.00,00. ,X (LENG+1) 
+ X (LENG+2) ) 

mam “πριιπί.Ι5..26..111..112.2) 

CALL AXIS(O. ,O. ,NAMEY,13,3.00,90. ,Y (LENG+1) , 
+ Y (LENG+2) ) 

CALL LINE (X,Y,LENG,1,0,3) 

CALL PLOT(0.0,0.0,999) 

RETURN 


hJ 


END 

SUBROUTINE ED 

CHARACTER*1 C1,C2,C3,C4A 

INTEGER*2 IC(4). 

EQUIVALENCE (C1,10010)),(€2, IC C200 ο In sim 

(C4,IC (4) ) 

DATA IC/16481B,16485B,164532,164844/ 

WRITE(*,1) C1,C2,C3,CA 

FORMAT (1X,441) 

RETURN 

END 

SUBROUTINE CUP (N,M). 

CHARACTER*1 C1,C2,C5,C8,LC(5) 

CHARACTER*5 CBUFF 

INTEGER*2 IC(4) 

EQUIVALENCE (C1,IC(1)),(C2,IC(2)), (CS,IC(3)), 
(CB8B,IC(4)), (CBUFF ,LC(1)) 

DATA 1Ι0/16818Β. 16858. 16858. 16866/ 

L-10000-1003N--M 

WRITE(CBUFF,2)L 

FORMAT (I5) 

WRITEOGK,1) C1,C2,LC(2) ,LLCCO) ασ EE ΕΞ ES 

FORMAT (1X,8A1,\) 

RETURN 

END 


112 


APPENDIX E 
BATGH PILES 


LISTING OF AUTOEXEC. BAT FILE ON OPERATING SYSTEH DISEkE: 
ECHO OFF 

GRAFHICS 

TIMER/S 

COPY A:RUN.BAT Cs: 

GORY AVKEDIT.EXE C:/¥V 

COPY" GerRGPILE.KED το. ν 

ms 

RUN 


LISTING OF RUN.BAT FILE ON VIRTUAL DISK(C) | 
ECHO. Insert the disk, which has the source file of the 
ECHO. sequential extended Kalman filter and Smoothing, 
ECHO. into drive A 

PAUSE 

CERN A: THESIS. FOR C: 

EEDIT C: THESIS. POR 

CORY C: THESIS. FOR A: 

ERASE C:KEDIT. EXE 

ERASE C:PROF TEE.KED 

ECHO. Insert the disk, which has PROFORT.EXE and 

ECHO. LINKE.EXE, into drive A, and the disk, which has 
ECHO. FROFORT.LIB into drive B. 

PAUSE 

A PROFORT THESIS /L /E 

A:LINK THESIS, , NULL , PROFORT 

ERASE C: THESIS. FOR 

ERASE Cs: THESIS. OBJ 

THESIS 

ECHO. Insert the disk, which has the source file of the 
ECHO. sequential extended Kalman filter and Smoothing. 
ECHO. into drive A, and the dist labeled "DATA" into 
ECHO. drive 日 。 

PAUSE 

COF EC: THESIS. EXE A: 

COPY C:*.DAT B: 

ERASE C:*.* 

ECHO. Insert the operating system disk into drive A, and 
ECHO. the disk, which has the plotting routine source 
ECHO. file into drive B. 

PAUSE 

CURT Ω:ΚΕΡΙΤ.ΕΧΕ C: 

Cer ose ROFILE.KED C: 

EDFY B:GRAPH.FOR C: 

KEDIT Cs: GRAPH. FOR 

OPY C: GRAPH.FOR B: 
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ERASE C:KEDIT.EXE 

ERASE C:PROFILE.KED | 

ECHO. Insert the disk, which has FORL.EXE and FAS2Z.EXE 
ECHO. into drive A, and the disk, which has PLOTSS.LIsB 
ECHO. into drive B. 

PAUSE 

A:FORI GRAFH; 

A:PAS2 | 

ECHO. Insert the disk, which has FORTRAN.LIB, ΜΑΤΗ.Ι ΤΕ 
ECHO. and LINK.EXE into drive ñ. 

PAUSE 

A:LINK GRAPH, ,NULL,B:PLOTTS8+A: FORTRAN+A: MATH 

ECHO. Insert the disk, which has the plotting source 
ECHO. file into drive A and the data disk into drive B. 
PAUSE š 

COPY C:GRAPH.EXE A: 

ERASE C:GRAPH.FOR 

ERASE C: GRAFH. OBJ 

COPY B: x DATIC: 

GRAPH 
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